Merge commit 'origin/org.openembedded.dev' into martin_jansa/xorg-7.5

Conflicts:
	conf/distro/include/angstrom-2008-preferred-versions.inc

1 files changed, 13031 insertions, 0 deletions

diff --git a/recipes/linux/linux-sgh-i900/sgh-i900-support.patch b/recipes/linux/linux-sgh-i900/sgh-i900-support.patch
new file mode 100644
index 0000000000..28d65938a1
--- /dev/null
+++ b/recipes/linux/linux-sgh-i900/sgh-i900-support.patch
@@ -0,0 +1,13031 @@
+diff -ur linux-2.6.32/arch/arm/Kconfig kernel/arch/arm/Kconfig
+--- linux-2.6.32/arch/arm/Kconfig	2009-12-03 05:51:21.000000000 +0200
++++ kernel/arch/arm/Kconfig	2009-12-12 16:09:25.656278659 +0200
+@@ -1502,6 +1502,112 @@
+ config ARCH_SUSPEND_POSSIBLE
+ 	def_bool y
+ 
++config PXA_DVFM
++	bool "PXA Processor High Level DVFM support"
++	depends on PM
++	default y
++	help
++	  This enables the dynamical frequency and voltage changes framework
++	  for PXA Processor series.
++
++config PXA_MIPSRAM
++	bool "PXA MIPSRAM monitoring support"
++	default n
++	help
++	  Enable MIPS RAM monitoring for process switching implemented in
++	  the scheduler
++
++config PXA3xx_DVFM
++	bool "PXA3xx Processor DVFM support"
++	depends on PM && PXA3xx && PXA_DVFM
++#	select PXA3xx_ARAVA
++#	select PXA3xx_MICCO
++	default y
++	help
++	  This implements the dynamical frequency and voltage changes features
++	  for PXA3xx Processor particularly.
++
++config PXA3xx_DVFM_STATS
++	bool "PXA3xx/PXA930 Processor DVFM Statistics support"
++	depends on PXA3xx_DVFM
++	select RELAY
++	select DEBUG_FS
++	default y
++	help
++	  This is used to collect statistics during the dynamic frequency
++	  and voltage changes
++
++config PXA3xx_PMU
++	bool "PXA3xx/PXA930 Processor PMU support"
++	default y
++	help
++	  PXA3xx/PXA930 provide Performance Monitor Unit to report
++	  CPU statistics info.
++
++config PXA3xx_PRM
++	bool "PXA3xx Processor Profiler Resource Manager"
++	depends on PXA3xx_DVFM && PXA3xx_PMU
++	default y
++	help
++	  This enables the PXA3xx Processor Profiler Resource Manager
++
++config IPM
++	bool "Marvell(R) Scalable Power Management Profiler"
++	depends on PXA3xx_PRM
++	default y
++	help
++	  Support Profiler of Marvell(R) Scalable Power Management
++
++config IPMC
++        bool "Marvell(R) Scalable Power Management Userspace Daemon"
++        depends on PXA3xx_PRM
++        default n 
++        help
++          Support Userspace Daemon of Marvell(R) Scalable Power Management
++
++config BPMD
++        bool "Borqs Scalable Power Management Kernel Daemon"
++        depends on PXA3xx_PRM
++        default y
++        help
++          Kernel Daemon of Borqs Scalable Power Management
++
++config TEST_BPMD
++        bool "Borqs Scalable Power Management Test Module"
++        depends on PXA3xx_PRM
++        default y
++        help
++          Test Module of Borqs Scalable Power Management
++
++config IPM_DEEPIDLE
++	bool "PXA3xx/PXA930 Processor Deep Idle support"
++	depends on IPM
++	default y
++	help
++	  This enables the kernel support for PXA3xx/PXA930
++	  Processor Deep Idle (D0CS Idle)
++
++config IPM_D2IDLE
++	bool "Support PXA3xx/PXA930 Processor D2 Mode as Idle"
++	depends on IPM && PXA_32KTIMER
++	default y
++	help
++	  This enables kernel support PXA3xx/PXA930 D2 idle
++
++config PERIPHERAL_STATUS
++	bool "Support list peripheral status of pm"
++	depends on PM
++	default y
++	help
++	  This enables kernel support peripheral status calculate
++
++config IPM_CGIDLE
++	bool "Support PXA935 Processor Clock Gated Mode as Idle"
++	depends on IPM && PXA_32KTIMER
++	default y
++	help
++	  This enables kernel support PXA935 D2 idle 
++
+ endmenu
+ 
+ source "net/Kconfig"
+diff -ur linux-2.6.32/arch/arm/mach-pxa/Kconfig kernel/arch/arm/mach-pxa/Kconfig
+--- linux-2.6.32/arch/arm/mach-pxa/Kconfig	2009-12-03 05:51:21.000000000 +0200
++++ kernel/arch/arm/mach-pxa/Kconfig	2009-12-12 16:09:26.426281936 +0200
+@@ -27,6 +27,12 @@
+ 	bool "PXA950 (codename Tavor-PV2)"
+ 	select CPU_PXA930
+ 
++config PXA3xx_PMIC
++        bool "PXA3xx PMIC support"
++	default y
++        help
++          PMIC support
++
+ endmenu
+ 
+ endif
+@@ -303,6 +309,18 @@
+ 	select HAVE_PWM
+ 	select PXA_HAVE_BOARD_IRQS
+ 
++config MACH_SGH_I900
++	bool "Samsung SGH-i900 (Omnia) phone"
++	select PXA3xx
++	select CPU_PXA310
++	select HAVE_PWM
++
++config MACH_SGH_I780
++	bool "Samsung SGH-i780 phone"
++	select PXA3xx
++	select CPU_PXA310
++	select HAVE_PWM
++
+ config MACH_LITTLETON
+ 	bool "PXA3xx Form Factor Platform (aka Littleton)"
+ 	select PXA3xx
+diff -ur linux-2.6.32/arch/arm/mach-pxa/Makefile kernel/arch/arm/mach-pxa/Makefile
+--- linux-2.6.32/arch/arm/mach-pxa/Makefile	2009-12-03 05:51:21.000000000 +0200
++++ kernel/arch/arm/mach-pxa/Makefile	2009-12-12 16:09:26.426281936 +0200
+@@ -5,6 +5,15 @@
+ # Common support (must be linked before board specific support)
+ obj-y				+= clock.o devices.o generic.o irq.o \
+ 				   time.o reset.o
++obj-$(CONFIG_PXA_DVFM)		+= dvfm.o
++ifeq ($(CONFIG_PXA3xx), y)
++  obj-$(CONFIG_PXA3xx_PMIC)	+= pxa3xx_pmic.o
++  obj-$(CONFIG_PXA3xx_DVFM)	+= pxa3xx_dvfm.o pxa3xx_dvfm_ll.o
++  obj-$(CONFIG_PXA3xx_PMU)	+= pmu.o pmu_ll.o
++  obj-$(CONFIG_PXA3xx_PRM)	+= prm.o
++  obj-$(CONFIG_BPMD)		+= bpm.o bpm_prof.o
++endif
++
+ obj-$(CONFIG_PM)		+= pm.o sleep.o standby.o
+ 
+ ifeq ($(CONFIG_CPU_FREQ),y)
+@@ -66,6 +75,8 @@
+ obj-$(CONFIG_MACH_PALMZ72)	+= palmz72.o
+ obj-$(CONFIG_MACH_TREO680)	+= treo680.o
+ obj-$(CONFIG_ARCH_VIPER)	+= viper.o
++obj-$(CONFIG_MACH_SGH_I900)	+= sgh_i780_i900.o sgh_smd.o sgh_rpc.o
++obj-$(CONFIG_MACH_SGH_I780)	+= sgh_i780_i900.o sgh_smd.o sgh_rpc.o
+ 
+ ifeq ($(CONFIG_MACH_ZYLONITE),y)
+   obj-y				+= zylonite.o
+diff -ur linux-2.6.32/arch/arm/mach-pxa/bpm.c kernel/arch/arm/mach-pxa/bpm.c
+--- linux-2.6.32/arch/arm/mach-pxa/bpm.c	2009-12-13 12:57:59.831957275 +0200
++++ kernel/arch/arm/mach-pxa/bpm.c	2009-12-12 16:09:26.429614458 +0200
+@@ -0,0 +1,1814 @@
++/*
++ * linux/arch/arm/mach-pxa/bpm.c
++ *
++ * Provide bpm thread to scale system voltage & frequency dynamically.
++ *
++ * Copyright (C) 2008 Borqs Corporation.
++ *
++ * Author:  Emichael Li <emichael.li@borqs.com>
++ *
++ * This software program is licensed subject to the GNU General Public License
++ * (GPL).Version 2,June 1991, available at http://www.fsf.org/copyleft/gpl.html
++ *
++ */
++
++#include <linux/kernel.h>
++#include <mach/prm.h>
++#include <mach/dvfm.h>
++#include <mach/mspm_prof.h>
++#include <linux/sysdev.h>
++#include <linux/delay.h>
++#include <mach/bpm.h>
++#include <mach/hardware.h>
++#include <mach/pxa3xx-regs.h>
++#include <linux/list.h>
++#include <asm/io.h>
++#include <asm/mach-types.h>
++#include <linux/freezer.h>
++#include <mach/regs-ost.h>
++#ifdef CONFIG_ANDROID_POWER
++#include <linux/android_power.h>
++#endif
++
++#define DEBUG
++
++#ifdef DEBUG
++#define PM_BUG_ON(condition)						      \
++	do { 								      \
++		if (unlikely(condition)) {				      \
++                        printk(KERN_ERR "BUG: failure at %s:%d/%s()!\n",      \
++					__FILE__, __LINE__, __FUNCTION__);    \
++			WARN_ON(1);  					      \
++		}							      \
++	} while(0)
++#define DPRINTK(fmt,args...) 						      \
++	do { 								      \
++		if (g_bpm_log_level) 					      \
++			printk(KERN_ERR "%s: " fmt, __FUNCTION__ , ## args);  \
++	} while (0)
++#else
++#define PM_BUG_ON(condition)                                                  \
++        do {                                                                  \
++                if (unlikely(condition)) {                                    \
++                        printk(KERN_ERR "BUG: failure at %s:%d/%s()!\n",      \
++                                        __FILE__, __LINE__, __FUNCTION__);    \
++                }                                                             \
++        } while(0)
++#define DPRINTK(fmt,args...) 						      \
++	do {} while (0)
++#endif
++
++/*****************************************************************************/
++/*                                                                           */
++/*                         Policy variables                                  */
++/*                                                                           */
++/*****************************************************************************/
++#define REDUCE_624M_DUTYCYCLE			(1)
++
++#define BPM_FREQ_POLICY_NUM			(3)
++#define BPM_PROFILER_WINDOW			(100)
++#define SYSTEM_BOOTUP_TIME			(15000)
++#define BPM_MAX_OP_NUM  			(10)
++
++struct bpm_freq_bonus_arg {
++	int mips;
++	int mem_stall;
++};
++
++struct bpm_freq_policy {
++	int lower[BPM_FREQ_POLICY_NUM];
++	int higher[BPM_FREQ_POLICY_NUM];
++};
++
++#define CONSTRAINT_ID_LEN			(32)
++struct bpm_cons {
++	struct list_head	list;
++	char	sid[CONSTRAINT_ID_LEN];
++	int	count;
++	unsigned long 	ms;
++	unsigned long 	tmp_ms;
++	unsigned long 	tm;
++};
++
++struct bpm_cons_head {
++        struct list_head        list;
++};
++
++/* manage all the ops which are supported by the hardware */
++static struct dvfm_op g_dyn_ops[BPM_MAX_OP_NUM];
++static spinlock_t g_dyn_ops_lock = SPIN_LOCK_UNLOCKED;
++
++static struct bpm_cons_head g_bpm_cons[BPM_MAX_OP_NUM];
++
++/* map the op from active ops to g_dyn_ops[] */
++static int g_active_ops_map[BPM_MAX_OP_NUM];
++static int g_active_ops_num;
++static int g_active_cur_idx = -1;
++static int g_prefer_op_idx;
++static int g_active_bonus[BPM_MAX_OP_NUM][BPM_MAX_OP_NUM * 2 - 1];
++struct bpm_freq_policy g_active_policy[BPM_MAX_OP_NUM];
++
++/*****************************************************************************/
++/*                                                                           */
++/*                     Framework Supportted Variables                        */
++/*                                                                           */
++/*****************************************************************************/
++
++int (*pipm_start_pmu) (void *) = NULL;
++EXPORT_SYMBOL(pipm_start_pmu);
++int (*pipm_stop_pmu)(void) = NULL;
++EXPORT_SYMBOL(pipm_stop_pmu);
++
++static int g_bpm_thread_exit;
++int g_bpm_enabled;
++static wait_queue_head_t g_bpm_enabled_waitq;
++
++static int g_profiler_window = BPM_PROFILER_WINDOW;
++static int g_bpm_log_level = 1;
++struct completion g_bpm_thread_over;
++
++extern struct sysdev_class cpu_sysdev_class;
++
++static struct bpm_event_queue g_bpm_event_queue;
++static spinlock_t g_bpm_event_queue_lock = SPIN_LOCK_UNLOCKED;
++
++#ifdef CONFIG_TEST_BPMD
++static int g_cpuload_mode;
++#endif
++
++static int dvfm_dev_idx;
++
++extern int __dvfm_enable_op(int index, int dev_idx);
++extern int __dvfm_disable_op2(int index, int dev_idx);
++extern int cur_op;
++extern struct info_head dvfm_trace_list;
++
++extern int g_dvfm_disabled;
++
++#ifdef CONFIG_MTD_NAND_HSS_FIX
++extern atomic_t nand_in_cmd;
++#endif
++/*****************************************************************************/
++/*                                                                           */
++/*                          Blink Variables                                  */
++/*                                                                           */
++/*****************************************************************************/
++#define DVFM_BLINK_OWNER_LEN (16)
++
++struct dvfm_blink_info {
++	int time;
++	char name[DVFM_BLINK_OWNER_LEN];
++};
++
++static int g_dvfm_blink = 0;
++static struct timer_list g_dvfm_blink_timer;
++static struct dvfm_blink_info g_dvfm_binfo;
++static unsigned long g_dvfm_blink_timeout = 0;
++
++/*****************************************************************************/
++/*                                                                           */
++/*                          android power interface                          */
++/*                                                                           */
++/*****************************************************************************/
++static int g_android_suspended = 0;
++
++#ifdef CONFIG_ANDROID_POWER
++void bpm_android_suspend_handler(android_early_suspend_t *h)
++{
++	unsigned long flags;
++	local_irq_save(flags);
++	g_android_suspended = 1;
++	local_irq_restore(flags);
++}
++
++void bpm_android_resume_handler(android_early_suspend_t *h)
++{
++	unsigned long flags;
++	local_irq_save(flags);
++	g_android_suspended = 0;
++	local_irq_restore(flags);
++}
++
++static android_early_suspend_t bpm_early_suspend = {
++	.level = 98,
++	.suspend = bpm_android_suspend_handler,
++	.resume = bpm_android_resume_handler,
++};
++#endif
++
++static inline int is_out_d0cs(void)
++{
++#ifdef CONFIG_PXA3xx_DVFM
++	extern int out_d0cs;
++	return out_d0cs;
++#endif
++	return 0;
++}
++
++/*****************************************************************************/
++/*                                                                           */
++/*                          BPMD Event Queue                                 */
++/*                                                                           */
++/*****************************************************************************/
++
++static int bpmq_init(void)
++{
++	g_bpm_event_queue.head = g_bpm_event_queue.tail = 0;
++	g_bpm_event_queue.len = 0;
++	init_waitqueue_head(&g_bpm_event_queue.waitq);
++	return 0;
++}
++
++static int bpmq_clear(void)
++{
++	unsigned long flag;
++
++	spin_lock_irqsave(&g_bpm_event_queue_lock, flag);
++
++	g_bpm_event_queue.head = g_bpm_event_queue.tail = 0;
++	g_bpm_event_queue.len = 0;
++
++	spin_unlock_irqrestore(&g_bpm_event_queue_lock, flag);
++
++	return 0;
++}
++
++static int bpmq_get(struct bpm_event *e)
++{
++	unsigned long flag;
++
++	spin_lock_irqsave(&g_bpm_event_queue_lock, flag);
++
++	if (!g_bpm_event_queue.len) {
++		spin_unlock_irqrestore(&g_bpm_event_queue_lock, flag);
++		printk(KERN_ERR "Logic error, please check bpmq_empty()\n");
++		return -1;
++	}
++	memcpy(e, g_bpm_event_queue.bpmes + g_bpm_event_queue.tail,
++	       sizeof(struct bpm_event));
++	g_bpm_event_queue.len--;
++	g_bpm_event_queue.tail =
++	    (g_bpm_event_queue.tail + 1) % MAX_BPM_EVENT_NUM;
++
++	spin_unlock_irqrestore(&g_bpm_event_queue_lock, flag);
++
++	return 0;
++}
++
++static int bpmq_put(struct bpm_event *e)
++{
++	unsigned long flag;
++	static int err_cnt = 0;
++
++	if (unlikely(0 == g_bpm_enabled))
++		return 0;
++
++	spin_lock_irqsave(&g_bpm_event_queue_lock, flag);
++
++	if (g_bpm_event_queue.len == MAX_BPM_EVENT_NUM) {
++		if (++err_cnt > 0) {
++			printk(KERN_ERR "bpm queue over flow!\n");
++			show_state();
++			printk(KERN_ERR "send event many times instantly?");
++			dump_stack();
++		}
++		spin_unlock_irqrestore(&g_bpm_event_queue_lock, flag);
++		return -1;
++	}
++	memcpy(g_bpm_event_queue.bpmes + g_bpm_event_queue.head, e,
++	       sizeof(struct bpm_event));
++	g_bpm_event_queue.len++;
++	g_bpm_event_queue.head =
++	    (g_bpm_event_queue.head + 1) % MAX_BPM_EVENT_NUM;
++
++	spin_unlock_irqrestore(&g_bpm_event_queue_lock, flag);
++
++	wake_up_interruptible(&g_bpm_event_queue.waitq);
++
++	return 0;
++}
++
++static __inline int bpmq_empty(void)
++{
++	return (g_bpm_event_queue.len > 0) ? 0 : 1;
++}
++
++int bpm_event_notify(int type, int kind, void *info, unsigned int info_len)
++{
++	struct bpm_event event;
++	int len = 0;
++
++	if (info_len > INFO_SIZE)
++		len = INFO_SIZE;
++	else if ((info_len < INFO_SIZE) && (info_len > 0))
++		len = info_len;
++	memset(&event, 0, sizeof(struct bpm_event));
++	event.type = type;
++	event.kind = kind;
++	if ((len > 0) && (info != NULL)) {
++		memcpy(event.info, info, len);
++	}
++	if (0 != bpmq_put(&event)) {
++		len = -1;
++	}
++
++/*	DPRINTK("type: %d kind: %d, len(ret): %d\n", type, kind, len); */
++	return len;
++}
++
++EXPORT_SYMBOL(bpm_event_notify);
++
++/*****************************************************************************/
++/*                                                                           */
++/*                               BPMD PMU Interface                          */
++/*                                                                           */
++/*****************************************************************************/
++
++static int bpm_start_pmu(void)
++{
++	int ret = -ENXIO;
++	struct ipm_profiler_arg pmu_arg;
++
++	if (pipm_start_pmu != NULL) {
++		pmu_arg.size = sizeof(struct ipm_profiler_arg);
++/*              pmu_arg.flags = IPM_IDLE_PROFILER | IPM_PMU_PROFILER; */
++		pmu_arg.flags = IPM_IDLE_PROFILER;
++		pmu_arg.window_size = g_profiler_window;
++
++		pmu_arg.pmn0 = PXA3xx_EVENT_EXMEM;
++		pmu_arg.pmn1 = PXA3xx_EVENT_DMC_NOT_EMPTY;
++		pmu_arg.pmn2 = PMU_EVENT_POWER_SAVING;
++		pmu_arg.pmn3 = PMU_EVENT_POWER_SAVING;
++
++		ret = pipm_start_pmu(&pmu_arg);
++	} else {
++		printk(KERN_CRIT "No profiler\n");
++		PM_BUG_ON(1);
++	}
++
++	return ret;
++}
++
++static int bpm_stop_pmu(void)
++{
++	pipm_stop_pmu();
++	return 0;
++}
++
++/*****************************************************************************/
++/*                                                                           */
++/*                               BPMD POLICY                                 */
++/*                                                                           */
++/*****************************************************************************/
++
++static int bpm_dump_policy(void)
++{
++#define TMP_BUF_SIZE (4096)
++	int i, j;
++	char *buf = kmalloc(TMP_BUF_SIZE, GFP_KERNEL);
++	char *s = NULL;
++
++	if (NULL == buf) {
++		printk(KERN_ERR "Can not alloc memory\n");
++		return 0;
++	}
++
++	s = buf;
++	memset(s, 0, TMP_BUF_SIZE);
++
++	s += sprintf(s, "--------------BPM DUMP POLICY BEGIN--------------\n");
++	s += sprintf(s, "dyn_boot_op = %d\n", dvfm_get_defop());
++	s += sprintf(s, "g_active_ops_maps:\n");
++
++	for (i = 0; i < BPM_MAX_OP_NUM; ++i)
++		s += sprintf(s, "%8d ", g_active_ops_map[i]);
++	s += sprintf(s, "\n");
++
++	s += sprintf(s, "g_active_ops_num: %d\n", g_active_ops_num);
++	s += sprintf(s, "g_active_cur_idx: %d\n", g_active_cur_idx);
++
++	s += sprintf(s, "g_active_policy:\n");
++	for (i = 0; i < BPM_MAX_OP_NUM; ++i) {
++		for (j = 0; j < BPM_FREQ_POLICY_NUM; ++j) {
++			s += sprintf(s, "%8d ", g_active_policy[i].lower[j]);
++		}
++
++		for (j = 0; j < BPM_FREQ_POLICY_NUM; ++j) {
++			s += sprintf(s, "%8d ", g_active_policy[i].higher[j]);
++		}
++		s += sprintf(s, "\n");
++	}
++
++	DPRINTK("%s", buf);
++
++	s = buf;
++	memset(s, 0, TMP_BUF_SIZE);
++
++	s += sprintf(s, "g_active_bonus:\n");
++	for (i = 0; i < BPM_MAX_OP_NUM; ++i) {
++		for (j = 0; j < BPM_MAX_OP_NUM * 2 - 1; ++j) {
++			s += sprintf(s, "%8d ", g_active_bonus[i][j]);
++		}
++		s += sprintf(s, "\n");
++	}
++
++	DPRINTK("%s", buf);
++
++	s = buf;
++	memset(s, 0, TMP_BUF_SIZE);
++
++	s += sprintf(s, "g_dyn_ops num: %d\n",
++		     sizeof(g_dyn_ops) / sizeof(struct dvfm_op));
++
++	s += sprintf(s, "g_dyn_ops:\n");
++
++	for (i = 0; i < sizeof(g_dyn_ops) / sizeof(struct dvfm_op); ++i) {
++		s += sprintf(s, "%8d %8d %8d %s\n",
++			     g_dyn_ops[i].index,
++			     g_dyn_ops[i].count,
++			     g_dyn_ops[i].cpu_freq, g_dyn_ops[i].name);
++	}
++	s += sprintf(s, "--------------BPM DUMP POLICY END----------------\n");
++
++	DPRINTK("%s", buf);
++
++	kfree(buf);
++	return 0;
++}
++
++static int build_active_ops(void)
++{
++	int i, j;
++	int pre_idx;
++	int cur_idx;
++	int pre_freq, cur_freq, pre_ratio;
++	int m, n;
++
++	memset(g_active_ops_map, -1, sizeof(g_active_ops_map));
++
++	for (i = 0, j = 0; i < BPM_MAX_OP_NUM; ++i) {
++		if (g_dyn_ops[i].count == 0 && g_dyn_ops[i].name != NULL
++		    && !dvfm_check_active_op(g_dyn_ops[i].index))
++			g_active_ops_map[j++] = i;
++	}
++
++	g_active_ops_num = j;
++	g_active_cur_idx = -1;
++
++	memset(g_active_bonus, -1, sizeof(g_active_bonus));
++	memset(g_active_policy, -1, sizeof(g_active_policy));
++
++	for (i = 0; i < g_active_ops_num; ++i) {
++		g_active_policy[i].higher[0] = 80;
++		g_active_policy[i].higher[1] = 95;
++		g_active_policy[i].higher[2] = 100;
++
++		if (i == 0) {
++			memset(g_active_policy[i].lower, 0,
++			       sizeof(g_active_policy[i].lower));
++			cur_idx = g_active_ops_map[i];
++			cur_freq = g_dyn_ops[cur_idx].cpu_freq;
++			if (cur_freq == 60) {
++				g_active_policy[i].higher[0] = 90;
++			}
++		} else {
++			pre_idx = g_active_ops_map[i - 1];
++			cur_idx = g_active_ops_map[i];
++			pre_freq = g_dyn_ops[pre_idx].cpu_freq;
++			cur_freq = g_dyn_ops[cur_idx].cpu_freq;
++			pre_ratio = g_active_policy[i - 1].higher[0];
++
++			g_active_policy[i].lower[2] = pre_freq * pre_ratio / cur_freq;
++
++			if (i > 1) {
++				pre_idx = g_active_ops_map[i - 2];
++				pre_freq = g_dyn_ops[pre_idx].cpu_freq;
++				pre_ratio = g_active_policy[i - 2].higher[0];
++
++				g_active_policy[i].lower[1] = pre_freq * pre_ratio / cur_freq; 
++			} else {
++				g_active_policy[i].lower[1] = 0;	
++			}
++
++			g_active_policy[i].lower[0] = 0;
++		}
++
++		for (j = 0; j < g_active_ops_num - 1 - i; ++j) {
++			g_active_bonus[i][j] = 0;
++		}
++
++		m = g_active_ops_num - 1;
++		n = 0;
++		for (j = m - i; j < 2 * g_active_ops_num - 1; ++j) {
++			g_active_bonus[i][j] = n < m ? n : m;
++			++n;
++		}
++
++	}
++
++	g_active_policy[i - 1].higher[0] = 100;
++	g_active_policy[i - 1].higher[1] = 100;
++	g_active_policy[i - 1].higher[2] = 100;
++
++#if REDUCE_624M_DUTYCYCLE
++	cur_idx = g_active_ops_map[i - 1];
++	cur_freq = g_dyn_ops[cur_idx].cpu_freq;
++	if (cur_freq == 624) {
++		if (i > 1) {
++			g_active_policy[i - 2].higher[0] = 96;
++			g_active_policy[i - 2].higher[1] = 100;
++
++			pre_idx = g_active_ops_map[i - 2];
++			pre_freq = g_dyn_ops[pre_idx].cpu_freq;
++			pre_ratio = g_active_policy[i - 2].higher[0];
++
++			g_active_policy[i - 1].lower[2] = pre_freq * pre_ratio / cur_freq; 
++		}
++		if (i > 2) {
++			g_active_policy[i - 3].higher[1] = 100;
++
++			pre_idx = g_active_ops_map[i - 3];
++			pre_freq = g_dyn_ops[pre_idx].cpu_freq;
++			pre_ratio = g_active_policy[i - 3].higher[0];
++
++			g_active_policy[i - 1].lower[1] = pre_freq * pre_ratio / cur_freq; 
++		}
++	}
++#endif
++	return 0;
++}
++
++/*****************************************************************************/
++/*                                                                           */
++/*                               Platform Related                            */
++/*                                                                           */
++/*****************************************************************************/
++
++int get_op_power_bonus(void)
++{
++	if (0 == g_active_cur_idx)
++		return 1;
++	else
++		return 0;
++}
++
++static int build_dyn_ops(void)
++{
++	int i;
++	int ret;
++	int op_num = 0;
++	int count, x;
++
++	struct op_info *info = NULL;
++	struct op_freq freq;
++
++	op_num = dvfm_op_count();
++	PM_BUG_ON(op_num > BPM_MAX_OP_NUM);
++
++	memset(&g_dyn_ops, -1, sizeof(g_dyn_ops));
++
++	for (i = 0; i < op_num; ++i) {
++		ret = dvfm_get_opinfo(i, &info);
++
++		PM_BUG_ON(ret);
++
++		/* calculate how much bits is set in device word */
++		x = info->device;
++		for (count = 0; x; x = x & (x - 1), count++);
++
++		g_dyn_ops[i].index = i;
++		g_dyn_ops[i].count = count;
++
++		ret = dvfm_get_op_freq(i, &freq);
++		PM_BUG_ON(ret);
++
++		g_dyn_ops[i].cpu_freq = freq.cpu_freq;
++
++		g_dyn_ops[i].name = dvfm_get_op_name(i);
++
++		PM_BUG_ON(!g_dyn_ops[i].name);
++
++		INIT_LIST_HEAD(&(g_bpm_cons[i].list));
++	}
++
++	for (i = op_num; i < BPM_MAX_OP_NUM; ++i) {
++		g_dyn_ops[i].index = -1;
++		g_dyn_ops[i].count = 0;
++		g_dyn_ops[i].cpu_freq = 0;
++		g_dyn_ops[i].name = NULL;
++
++		INIT_LIST_HEAD(&(g_bpm_cons[i].list));
++	}
++
++	return 0;
++}
++
++static int get_dyn_idx(int active_idx)
++{
++	int t;
++	t = g_active_ops_map[active_idx];
++	return g_dyn_ops[t].index;
++}
++
++static int get_cur_freq(void)
++{
++	PM_BUG_ON(g_active_cur_idx == -1);
++	return g_dyn_ops[get_dyn_idx(g_active_cur_idx)].cpu_freq;
++}
++
++static int calc_new_idx(int bonus)
++{
++	int new_idx;
++
++	new_idx =
++	    g_active_bonus[g_active_cur_idx][bonus + g_active_ops_num - 1];
++
++	return new_idx;
++}
++
++static int calc_bonus(struct bpm_freq_bonus_arg *parg)
++{
++	int i;
++	int bonus = 0;
++	int mem_stall = parg->mem_stall;
++	int mipsload = parg->mips * 100 / get_cur_freq();
++	int cpuload =  mipsload > 100 ? 100 : mipsload;
++
++	PM_BUG_ON(cpuload > 100 || cpuload < 0);
++
++	for (i = 0; i < BPM_FREQ_POLICY_NUM; ++i) {
++		if (cpuload > g_active_policy[g_active_cur_idx].higher[i]) {
++			bonus += 1;
++//			break;	/* FIX ME: change the freq one by one */
++		}
++	}
++
++	for (i = BPM_FREQ_POLICY_NUM - 1; i >= 0; --i) {
++		if (cpuload < g_active_policy[g_active_cur_idx].lower[i]) {
++			bonus -= 1;
++//			break;	/* FIX ME: change the freq one by one */
++		}
++	}
++
++	/* memory bound */
++	if (bonus <= 0 && mem_stall > 17)
++		bonus = 1;
++
++	/* change to user_sleep policy ... */
++	if (g_android_suspended && (g_active_cur_idx <= 1))
++		bonus -= 1;
++
++	if (bonus > g_active_ops_num - 1)
++		bonus = g_active_ops_num - 1;
++	else if (bonus < 1 - g_active_ops_num)
++		bonus = 1 - g_active_ops_num;
++
++	return bonus;
++}
++
++/*****************************************************************************/
++/*                                                                           */
++/*                               BPMD API                                    */
++/*                                                                           */
++/*****************************************************************************/
++
++static int bpm_change_op(int cur_idx, int new_idx)
++{
++	int ret;
++	struct dvfm_freqs freqs;
++	unsigned int oscr;
++
++	freqs.old = cur_idx;
++	freqs.new = new_idx;
++	oscr = OSCR;
++	ret = dvfm_set_op(&freqs, freqs.new, RELATION_STICK);
++	oscr = OSCR - oscr;
++	DPRINTK("old: %d cur: %d (tm: %d)\n", cur_idx, new_idx, oscr/325);
++/*	
++	DPRINTK("ACCR: 0x%x ACSR: 0x%x AVCR: 0x%x SVCR: 0x%x CVCR: 0x%x\n",
++			ACCR, ACSR, AVCR, SVCR, CVCR);
++*/
++	return ret;
++}
++
++/* this function need to be refatored later? */
++int bpm_disable_op(int dyn_idx, int dev_idx)
++{
++	int i;
++	int ret = 0;
++	int cur_op_idx = -1, op_idx;
++	int next_op_idx = -1, next_active_idx = -1;
++
++	op_idx = g_dyn_ops[dyn_idx].index;
++
++        /* save current op information */
++        if (g_active_cur_idx != -1) {
++                cur_op_idx = get_dyn_idx(g_active_cur_idx);
++        }
++
++        if (!dvfm_check_active_op(op_idx) && g_active_ops_num == 1 &&
++			cur_op_idx == op_idx) {
++		printk(KERN_ERR "Can't disable this op %d\n", op_idx);
++		bpm_dump_policy();
++		return -1;
++        }
++
++	/*
++ 	 * it should be at least two enabled ops here, 
++ 	 * otherwise it cannot come here if there is one enabled op. 
++ 	 */
++	if ((g_active_cur_idx != -1) && (g_active_ops_num > 1)) {
++                if (g_active_cur_idx == (g_active_ops_num - 1)) {
++                        next_op_idx = get_dyn_idx(g_active_cur_idx - 1);
++                        PM_BUG_ON((g_active_cur_idx - 1) < 0);
++                        if ((g_active_cur_idx - 1) < 0) {
++                                printk(KERN_ERR "err: %d %d\n", g_active_cur_idx, g_active_ops_num);
++                                bpm_dump_policy();
++                        }
++                } else {
++                        next_op_idx = get_dyn_idx(g_active_cur_idx + 1);
++                        PM_BUG_ON((g_active_cur_idx + 1) > (g_active_ops_num - 1));
++                        if ((g_active_cur_idx + 1) > (g_active_ops_num - 1)) {
++                                printk(KERN_ERR "err2: %d %d\n", g_active_cur_idx, g_active_ops_num);
++                                bpm_dump_policy();
++                        }
++                }		
++	}
++	
++	g_dyn_ops[dyn_idx].count++;
++
++	__dvfm_disable_op2(op_idx, dev_idx);
++
++	if (!dvfm_check_active_op(op_idx) && g_dyn_ops[dyn_idx].count == 1) {
++		build_active_ops();
++	}
++
++	if (cur_op_idx != -1) {
++        	for (i = 0; i < g_active_ops_num; ++i) {
++			if (get_dyn_idx(i) == cur_op_idx) {
++                		g_active_cur_idx = i;
++	                        break;
++        	        }
++       	 	}
++
++		/* the disabled op is previous op, change to another op */
++        	if (g_active_cur_idx == -1) {
++
++			/* find next op */
++                	for (i = 0; i < g_active_ops_num; ++i) {
++                        	if (get_dyn_idx(i) == next_op_idx) {
++                                	next_active_idx = i;
++	                                break;
++        	                }
++                	}
++
++              		PM_BUG_ON(cur_op_idx != op_idx);
++			PM_BUG_ON(next_op_idx != get_dyn_idx(next_active_idx));
++			g_active_cur_idx = next_active_idx;
++              		ret = bpm_change_op(cur_op_idx, next_op_idx); 
++			PM_BUG_ON(ret);
++       		}
++	}
++
++	return ret;
++}
++
++int bpm_enable_op(int dyn_idx, int dev_idx)
++{
++	int i, cur_op_idx = -1;
++
++	if (g_dyn_ops[dyn_idx].count <= 0) {
++		printk(KERN_ERR "are you disable this op before?\n");
++		return -1;
++	}
++
++        /* save current op information */
++        if (g_active_cur_idx != -1) {
++                cur_op_idx = get_dyn_idx(g_active_cur_idx);
++        }
++
++	g_dyn_ops[dyn_idx].count--;
++
++	if (g_dyn_ops[dyn_idx].count == 0)
++		build_active_ops();
++
++	__dvfm_enable_op(g_dyn_ops[dyn_idx].index, dev_idx);
++
++        if (cur_op_idx != -1) {
++                for (i = 0; i < g_active_ops_num; ++i) {
++                        if (get_dyn_idx(i) == cur_op_idx) {
++                                g_active_cur_idx = i;
++                                break;
++                        }
++                }
++	}
++
++	return 0;
++}
++
++int bpm_enable_op_name(char *name, int dev_idx, char *sid)
++{
++	unsigned long flag;
++	int ret = 0, new_idx = -1;
++	int i, found;
++	struct list_head *list = NULL;
++	struct bpm_cons *p = NULL;
++
++	spin_lock_irqsave(&g_dyn_ops_lock, flag);
++
++	for (i = 0; i < sizeof(g_dyn_ops) / sizeof(struct dvfm_op); ++i) {
++                if (g_dyn_ops[i].name != NULL && 
++                        (!strncmp(name, g_dyn_ops[i].name, sizeof(name)))) {
++			ret = bpm_enable_op(i, dev_idx);
++
++                        if (!ret) {
++				found = 0;
++                                list_for_each(list, &(g_bpm_cons[i].list)) {
++                                        p = list_entry(list, struct bpm_cons, list);
++                                        if (!strncmp(p->sid, sid, CONSTRAINT_ID_LEN - 1)) {
++						found = 1;
++						PM_BUG_ON(p->count <= 0);
++                                                p->count--;
++						if (p->tmp_ms) {
++							p->tm++;
++							p->ms += (OSCR / 3250 - p->tmp_ms);
++						}
++						break;
++                                        }
++                                }
++				PM_BUG_ON(!found);
++			} else {
++				printk(KERN_ERR "%s use PM interface rightly!\n", sid);
++				PM_BUG_ON(1);
++			}
++			break;
++		}
++	}
++
++	if (i == sizeof(g_dyn_ops) / sizeof(struct dvfm_op)) {
++//              printk(KERN_ERR "Cannot find and enable op name %s\n", name);
++	}
++
++	PM_BUG_ON((-1 != g_active_cur_idx) && (get_dyn_idx(g_active_cur_idx) != cur_op));	
++
++	/* Change to prefrer op */
++	if (g_prefer_op_idx != cur_op && g_active_cur_idx != -1) {
++		for (i = 0; i < g_active_ops_num; ++i) {
++			if (get_dyn_idx(i) == g_prefer_op_idx) {
++				new_idx = i;
++				break;
++			}
++		}
++
++		if (new_idx != -1) {
++			ret = bpm_change_op(get_dyn_idx(g_active_cur_idx), get_dyn_idx(new_idx));
++			if (0 == ret)
++				g_active_cur_idx = new_idx;
++			PM_BUG_ON((-1 != g_active_cur_idx) && (get_dyn_idx(g_active_cur_idx) != cur_op));
++		}
++	}
++
++	spin_unlock_irqrestore(&g_dyn_ops_lock, flag);
++
++	return ret;
++}
++
++int bpm_disable_op_name(char *name, int dev_idx, char *sid)
++{
++	unsigned long flag;
++	int ret = -1;
++	int i;
++	int find = 0;
++	struct list_head *list = NULL;
++	struct bpm_cons *p = NULL;
++
++	spin_lock_irqsave(&g_dyn_ops_lock, flag);
++
++	for (i = 0; i < sizeof(g_dyn_ops) / sizeof(struct dvfm_op); ++i) {
++		if (g_dyn_ops[i].name != NULL && 
++			(!strncmp(name, g_dyn_ops[i].name, sizeof(name)))) {
++			ret = bpm_disable_op(i, dev_idx);
++			
++			if (!ret) {
++	        		list_for_each(list, &(g_bpm_cons[i].list)) {
++					p = list_entry(list, struct bpm_cons, list);
++					if (!strncmp(p->sid, sid, CONSTRAINT_ID_LEN - 1)) {
++						p->count++;
++						p->tmp_ms = OSCR / 3250;
++						find = 1;
++						break;
++					}
++				}
++				
++				if (find == 0) {
++			                p = (struct bpm_cons *)kzalloc(sizeof(struct bpm_cons), GFP_KERNEL);
++					strncpy(p->sid, sid, CONSTRAINT_ID_LEN - 1);
++					p->count = 1;
++					list_add_tail(&(p->list), &(g_bpm_cons[i].list));
++				}
++			}
++			break;
++		}
++	}
++
++	if (i == sizeof(g_dyn_ops) / sizeof(struct dvfm_op)) {
++//              printk(KERN_ERR "Cannot find and disable op name %s\n", name);
++	}
++
++	PM_BUG_ON((-1 != g_active_cur_idx) && (get_dyn_idx(g_active_cur_idx) != cur_op));	
++
++	spin_unlock_irqrestore(&g_dyn_ops_lock, flag);
++
++	return ret;
++}
++
++static int handle_profiler_arg(struct bpm_freq_bonus_arg *parg)
++{
++	int bonus;
++	int new_idx;
++	unsigned long flag;
++	int cur_dyn_idx, new_dyn_idx;
++
++	if (g_dvfm_blink)
++		return 0;
++
++	/*
++	 * bpm_enable_op_name() and bpm_disable_op_name() will update
++	 * g_dyn_ops[] and g_active_xxx[], and then scale the op, so
++	 * we need to avoid the conflict.
++	 * Below code can not call schedule() indirectly.
++	 */
++	spin_lock_irqsave(&g_dyn_ops_lock, flag);
++
++	if (0 == g_bpm_enabled) {
++		spin_unlock_irqrestore(&g_dyn_ops_lock, flag);	
++		return 0;
++	}
++
++	bonus = calc_bonus(parg);
++	new_idx = calc_new_idx(bonus);
++
++        cur_dyn_idx = get_dyn_idx(g_active_cur_idx);
++        new_dyn_idx = get_dyn_idx(new_idx);
++
++/*
++	DPRINTK
++	    ("bonus:%d, cur_idx: %d, new_idx: %d, old_hw_idx: %d, new_hw_idx: %d\n",
++	     bonus, g_active_cur_idx, new_idx, cur_dyn_idx, new_dyn_idx);
++*/
++	if (new_idx != g_active_cur_idx) {
++		if (!bpm_change_op(cur_dyn_idx, new_dyn_idx)) {
++			g_active_cur_idx = new_idx;
++		} else {
++			DPRINTK("scaling freq later!\n");
++		}
++		g_prefer_op_idx = new_dyn_idx;
++	}
++
++	PM_BUG_ON((-1 != g_active_cur_idx) && (get_dyn_idx(g_active_cur_idx) != cur_op));	
++
++	spin_unlock_irqrestore(&g_dyn_ops_lock, flag);
++
++	return 0;
++}
++
++static void dvfm_blink_timer_handler(unsigned long data)
++{
++	unsigned long flag;
++
++	local_irq_save(flag);
++
++	g_dvfm_blink = 0;
++	g_dvfm_blink_timeout = 0;
++	memset(&g_dvfm_binfo, 0, sizeof(struct dvfm_blink_info));
++
++	local_irq_restore(flag);
++}
++
++static int handle_blink(struct bpm_event *pevent)
++{
++	int new_idx;
++	unsigned long flag;
++	int cur_dyn_idx, new_dyn_idx;
++	struct dvfm_blink_info *pinfo = NULL; 
++
++	if (0 == g_bpm_enabled)
++		return 0;
++
++	spin_lock_irqsave(&g_dyn_ops_lock, flag);
++
++	pinfo = (struct dvfm_blink_info *)pevent->info;
++
++	DPRINTK("Blink: %d %lu %lu\n", g_dvfm_blink, g_dvfm_blink_timeout, jiffies + msecs_to_jiffies(pinfo->time));
++
++	if ((0 == g_dvfm_blink) || time_before(g_dvfm_blink_timeout, jiffies + msecs_to_jiffies(pinfo->time))) {
++
++		memcpy(&g_dvfm_binfo, pinfo, sizeof(struct dvfm_blink_info));
++
++		g_dvfm_blink_timeout = jiffies + msecs_to_jiffies(pinfo->time);
++		g_dvfm_blink = 1;
++		mod_timer(&g_dvfm_blink_timer, g_dvfm_blink_timeout);
++
++		new_idx = g_active_ops_num - 1;
++		cur_dyn_idx = get_dyn_idx(g_active_cur_idx);
++		new_dyn_idx = get_dyn_idx(new_idx);
++
++		if (new_dyn_idx > cur_dyn_idx) {
++			if (!bpm_change_op(cur_dyn_idx, new_dyn_idx)) {
++				g_active_cur_idx = new_idx;
++				g_prefer_op_idx = new_dyn_idx;
++			}
++		}
++	} else {
++		printk("Blink: %s already set and blink(%lu)\n", g_dvfm_binfo.name, g_dvfm_blink_timeout);
++	}
++	 
++	PM_BUG_ON((-1 != g_active_cur_idx) && (get_dyn_idx(g_active_cur_idx) != cur_op));	
++
++	spin_unlock_irqrestore(&g_dyn_ops_lock, flag);
++
++	return 0;
++}
++
++static int handle_profiler(struct bpm_event *pevent)
++{
++	struct ipm_profiler_result *pinfo =
++	    (struct ipm_profiler_result *)pevent->info;
++	struct bpm_freq_bonus_arg bonus_arg;
++	int mips = pinfo->mips;
++	int mem_stall = 0;
++
++#ifdef CONFIG_TEST_BPMD
++	static int cpuload = 10;
++	switch (g_cpuload_mode) {
++	case 0:
++		cpuload = mips * 100 / get_cur_freq();
++		break;
++	case 1:
++		cpuload = (cpuload == 10 ? 90 : 10);
++		break;
++	case 2:
++		cpuload = OSCR % 101;
++		break;
++	case 3:
++		cpuload = (OSCR & 0x1) ? 90 : 10;
++		break;
++	case 4:
++		cpuload = OSCR % 21;
++		break;
++	case 5:
++		cpuload = 80 + OSCR % 21;
++		break;
++	}
++	mips = cpuload * get_cur_freq() / 100;
++
++//	DPRINTK("orig ratio: %d new ratio: %d\n", pinfo->busy_ratio, busy);
++#endif
++	DPRINTK("time_load: %d mips_load: %d (%d)\n", pinfo->busy_ratio, mips * 100 / get_cur_freq(), get_cur_freq());
++
++	/*
++	 * Get PMU Data, bla bla bla...  
++	 */
++	bonus_arg.mips = mips; 
++	bonus_arg.mem_stall = mem_stall;
++
++	handle_profiler_arg(&bonus_arg);
++
++	bpm_start_pmu();
++	return 0;
++}
++
++static int bpm_process_event(struct bpm_event *pevent)
++{
++	switch (pevent->type) {
++	case IPM_EVENT_PROFILER:
++		handle_profiler(pevent);
++		break;
++
++	case IPM_EVENT_BLINK:
++		handle_blink(pevent);
++		break;
++
++	default:
++		PM_BUG_ON(1);
++	}
++	return 0;
++}
++
++int bpm_pre_enter_d0csidle(int* op)
++{
++	unsigned long flag;
++	int ret = 0, new_dyn_idx;;
++
++	spin_lock_irqsave(&g_dyn_ops_lock, flag);
++
++	if (g_active_cur_idx != -1)
++		*op = get_dyn_idx(g_active_cur_idx);
++	else 
++		*op = dvfm_get_defop(); 
++
++	new_dyn_idx = get_dyn_idx(0);
++	if (*op > new_dyn_idx) {
++		ret = bpm_change_op(*op, new_dyn_idx);
++
++		if ((0 == ret) && (-1 != g_active_cur_idx)) {
++			g_active_cur_idx = 0;
++		}
++	}
++
++	PM_BUG_ON((-1 != g_active_cur_idx) && (get_dyn_idx(g_active_cur_idx) != cur_op));	
++
++	spin_unlock_irqrestore(&g_dyn_ops_lock, flag);
++
++#ifdef CONFIG_MTD_NAND_HSS_FIX	
++	if (!atomic_read(&nand_in_cmd))
++#endif
++		PM_BUG_ON(ret);
++
++	return ret;
++}
++
++int bpm_post_exit_d0csidle(int op)
++{
++	unsigned long flag;
++	int new_idx = -1;
++	int cur_dyn_op, new_dyn_op;
++	int i, ret;
++
++	spin_lock_irqsave(&g_dyn_ops_lock, flag);
++
++	if (g_active_cur_idx != -1) {
++		for (i = 0; i < g_active_ops_num; ++i) {
++			if (get_dyn_idx(i) >= op) {
++				new_idx = i;
++				break;
++			}
++		}
++
++		PM_BUG_ON(new_idx == -1);
++
++		cur_dyn_op = get_dyn_idx(g_active_cur_idx);
++		new_dyn_op = get_dyn_idx(new_idx);
++
++		PM_BUG_ON(cur_dyn_op != cur_op);
++
++		g_active_cur_idx = new_idx;
++	} else {
++		cur_dyn_op = cur_op;
++		new_dyn_op = dvfm_get_defop();
++		PM_BUG_ON(op != new_dyn_op);
++	}
++
++	PM_BUG_ON(cur_dyn_op > new_dyn_op);
++
++	if (cur_dyn_op != new_dyn_op) {
++		ret = bpm_change_op(cur_dyn_op, new_dyn_op);
++		PM_BUG_ON(ret);
++	}
++
++	PM_BUG_ON((-1 != g_active_cur_idx) && (get_dyn_idx(g_active_cur_idx) != cur_op));	
++
++	spin_unlock_irqrestore(&g_dyn_ops_lock, flag);
++
++	return 0;
++}
++
++int bpm_set_active_op(const unsigned char* opname)
++{
++	int opname_idx = -1, i, cur_idx;
++	int ret = 0;
++	unsigned long flag;
++
++	if (-1 != g_active_cur_idx) {
++		spin_lock_irqsave(&g_dyn_ops_lock, flag);	
++
++		for (i = 0; i < g_active_ops_num; ++i) {
++			cur_idx = g_active_ops_map[i];
++			if (!strcmp(opname, g_dyn_ops[cur_idx].name)) {
++				opname_idx = i;
++			}
++		}
++
++		if(opname_idx != -1) {
++			if (g_active_cur_idx != opname_idx) {
++				ret = bpm_change_op(get_dyn_idx(g_active_cur_idx), get_dyn_idx(opname_idx));
++				g_active_cur_idx = opname_idx;
++				g_prefer_op_idx = get_dyn_idx(opname_idx);
++				PM_BUG_ON(ret);
++			}
++		} else 
++			printk(KERN_WARNING "Cannot find %s, %s is disabled?\n", opname, opname);
++
++		PM_BUG_ON((-1 != g_active_cur_idx) && (get_dyn_idx(g_active_cur_idx) != cur_op));	
++
++		spin_unlock_irqrestore(&g_dyn_ops_lock, flag);
++	}
++
++	return ret;
++}
++/*****************************************************************************/
++/*                                                                           */
++/*                               BPMD Thread                                 */
++/*                                                                           */
++/*****************************************************************************/
++
++static int change_to_active_op(void)
++{
++	unsigned long flag;
++	int ret = 0;
++
++	spin_lock_irqsave(&g_dyn_ops_lock, flag);
++
++	g_active_cur_idx = g_active_ops_num - 1;
++	ret = bpm_change_op(dvfm_get_defop(), get_dyn_idx(g_active_cur_idx));
++	g_prefer_op_idx = cur_op; 
++
++	PM_BUG_ON((-1 != g_active_cur_idx) && (get_dyn_idx(g_active_cur_idx) != cur_op));	
++
++	spin_unlock_irqrestore(&g_dyn_ops_lock, flag);
++
++	PM_BUG_ON(ret);
++
++	return ret;
++}
++
++static int change_to_def_op(void)
++{
++	unsigned long flag;
++	int ret = 0;
++
++	spin_lock_irqsave(&g_dyn_ops_lock, flag);
++
++	ret = bpm_change_op(get_dyn_idx(g_active_cur_idx), dvfm_get_defop());
++	g_prefer_op_idx = cur_op;
++
++	g_active_cur_idx = -1;
++
++	spin_unlock_irqrestore(&g_dyn_ops_lock, flag);
++
++	PM_BUG_ON(ret);
++
++	return ret;
++}
++
++static int bpm_start(void)
++{
++	int ret;
++
++	if (0 == g_bpm_enabled) {
++		bpmq_clear();
++		change_to_active_op();
++		ret = bpm_start_pmu();
++		if (ret) {
++			printk(KERN_ERR "Can't start_pmu, ret: %d\n", ret);
++			g_bpm_enabled = 0;
++			return ret;
++		}
++		g_bpm_enabled = 1;
++#ifdef DEBUG
++		bpm_dump_policy();
++#endif
++		wake_up_interruptible(&g_bpm_enabled_waitq);
++	} else {
++		printk(KERN_DEBUG "bpmd already enabled (%d)\n", g_bpm_enabled);
++	}
++
++	return 0;
++}
++
++extern int gpio_reset_work_around(void);
++static int bpm_stop(void)
++{
++	if (1 == g_bpm_enabled) {
++		bpm_stop_pmu();
++		if (machine_is_bstd())
++			gpio_reset_work_around();
++		else
++			change_to_def_op();
++		g_bpm_enabled = 0;
++	} else {
++		printk(KERN_DEBUG "bpmd already stopped (%d)\n", g_bpm_enabled);
++	}
++
++	return 0;
++}
++
++static int bpm_thread(void *data)
++{
++	int ret = 0;
++	struct bpm_event event;
++	struct task_struct *tsk = current;
++	struct sched_param param = {.sched_priority = 1 };
++
++	DEFINE_WAIT(wait);
++	
++	if (g_dvfm_disabled)
++		goto thread_over;
++
++	daemonize("bpmd");
++	strcpy(tsk->comm, "bpmd");
++
++	allow_signal(SIGKILL);
++	sched_setscheduler(tsk, SCHED_FIFO, &param);
++
++	g_bpm_log_level = 0;
++
++	msleep(SYSTEM_BOOTUP_TIME);
++
++	ret = bpm_start();
++	PM_BUG_ON(ret);
++
++	DPRINTK("Begining bpm deamon thread ...\n");
++
++	while (likely(!g_bpm_thread_exit)) {
++
++		if (unlikely(signal_pending(tsk))) {
++			printk(KERN_NOTICE "BPMD is killed by SIGKILL!\n");
++			break;
++		}
++
++//		DPRINTK("g_bpm_enabled = %d, bpmq_empty = %d\n",
++//			g_bpm_enabled, bpmq_empty());
++
++		if (likely(g_bpm_enabled)) {
++			if (likely(bpmq_empty())) {
++				prepare_to_wait(&g_bpm_event_queue.waitq, &wait,
++						TASK_INTERRUPTIBLE);
++				schedule();
++				finish_wait(&g_bpm_event_queue.waitq, &wait);
++			}
++
++			if (likely(!bpmq_empty())) {
++				ret = bpmq_get(&event);
++				PM_BUG_ON(ret);
++
++				bpm_process_event(&event);
++			}
++		} else {
++			prepare_to_wait(&g_bpm_enabled_waitq, &wait,
++					TASK_INTERRUPTIBLE);
++			schedule();
++			finish_wait(&g_bpm_enabled_waitq, &wait);
++		}
++	}
++
++	bpm_stop();
++
++thread_over:	
++	complete_and_exit(&g_bpm_thread_over, 0);
++
++	printk(KERN_WARNING "bpm daemon thread exit!\n");
++	return 0;
++}
++
++/*****************************************************************************/
++/*                                                                           */
++/*                        BPMD SYS Interface                                 */
++/*                                                                           */
++/*****************************************************************************/
++
++static ssize_t op_show(struct sys_device *sys_dev, char *buf)
++{
++	int cur_dyn_idx, len;
++
++	if (g_active_cur_idx != -1)
++		cur_dyn_idx = get_dyn_idx(g_active_cur_idx);
++	else
++		cur_dyn_idx = dvfm_get_defop();
++
++	PM_BUG_ON(cur_dyn_idx != cur_op);
++
++	len = dvfm_dump_op(cur_dyn_idx, buf);
++
++	return len;
++}
++
++static ssize_t op_store(struct sys_device *sys_dev, const char *buf, size_t len)
++{
++	int i;
++	int dyn_idx, new_dyn_idx, cur_dyn_idx, new_active_idx = -1;
++	unsigned long flag;
++	int res = 0;
++
++	sscanf(buf, "%u", &new_dyn_idx);
++
++	spin_lock_irqsave(&g_dyn_ops_lock, flag);
++
++	for (i = 0; i < g_active_ops_num; ++i) {
++		dyn_idx = g_active_ops_map[i];
++		if (g_dyn_ops[dyn_idx].index == new_dyn_idx) {
++			new_active_idx = i;
++			break;
++		}
++	}
++
++	if (new_active_idx != -1) {
++	        if (g_active_cur_idx != -1)
++                	cur_dyn_idx = get_dyn_idx(g_active_cur_idx);
++        	else
++	                cur_dyn_idx = dvfm_get_defop();
++
++		res = bpm_change_op(cur_dyn_idx, new_dyn_idx);
++		g_prefer_op_idx = new_dyn_idx;
++
++		PM_BUG_ON(res);
++
++		g_active_cur_idx = new_active_idx;
++	} else {
++		printk(KERN_ERR "bpm is enabled, new dyn op:%d\n", new_dyn_idx);
++		printk(KERN_ERR "Cannot find new active op, please check it\n");
++	}
++
++	PM_BUG_ON((-1 != g_active_cur_idx) && (get_dyn_idx(g_active_cur_idx) != cur_op));	
++
++	spin_unlock_irqrestore(&g_dyn_ops_lock, flag);
++
++	return len;
++}
++
++SYSDEV_ATTR(op, 0644, op_show, op_store);
++
++static ssize_t ops_show(struct sys_device *sys_dev, char *buf)
++{
++	int len = 0;
++	char *p = NULL;
++	int i;
++
++	for (i = 0; i < sizeof(g_dyn_ops) / sizeof(struct dvfm_op); ++i) {
++		if (g_dyn_ops[i].name != NULL) {
++			p = buf + len;
++			len += dvfm_dump_op(i, p);
++		}
++	}
++
++	return len;
++}
++
++SYSDEV_ATTR(ops, 0444, ops_show, NULL);
++
++static ssize_t enable_op_show(struct sys_device *sys_dev, char *buf)
++{
++	int len = 0;
++	char *p = NULL;
++	int i;
++
++	for (i = 0; i < sizeof(g_dyn_ops) / sizeof(struct dvfm_op); ++i) {
++		if ((!g_dyn_ops[i].count) && (g_dyn_ops[i].name != NULL)) {
++			p = buf + len;
++			len += dvfm_dump_op(i, p);
++		}
++	}
++
++	return len;
++}
++
++static ssize_t enable_op_store(struct sys_device *sys_dev, const char *buf,
++			       size_t len)
++{
++	int level;
++	char name[16];
++
++	if (len >= 16) {
++		printk(KERN_ERR "invalid parameter\n");
++		return len;
++	}
++
++	memset(name, 0, sizeof(name));
++	sscanf(buf, "%s %d", name, &level);
++
++	if (level)
++		bpm_enable_op_name(name, dvfm_dev_idx, "user-echo");
++	else
++		bpm_disable_op_name(name, dvfm_dev_idx, "user-echo");
++
++	return len;
++}
++
++SYSDEV_ATTR(enable_op, 0666, enable_op_show, enable_op_store);
++
++static ssize_t profiler_window_show(struct sys_device *sys_dev, char *buf)
++{
++	char *s = buf;
++
++	s += sprintf(s, "%d\n", g_profiler_window);
++
++	return (s - buf);
++}
++
++static ssize_t profiler_window_store(struct sys_device *sys_dev,
++				     const char *buf, size_t n)
++{
++	sscanf(buf, "%u", &g_profiler_window);
++
++	if (g_profiler_window < 10 || g_profiler_window > 20000)
++		printk(KERN_ERR "please input the value in (10, 20000]\n");
++
++	return n;
++}
++
++SYSDEV_ATTR(profiler_window, 0644, profiler_window_show, profiler_window_store);
++
++static ssize_t bpm_show(struct sys_device *sys_dev, char *buf)
++{
++	char *s = buf;
++
++	if (g_bpm_enabled) 
++		s += sprintf(s, "%s\n", "enabled");
++	else 
++		s += sprintf(s, "%s\n", "disabled");
++
++	return (s - buf);
++}
++
++static ssize_t bpm_store(struct sys_device *sys_dev, const char *buf, size_t n)
++{
++	if (n >= strlen("enable") &&
++	    strncmp(buf, "enable", strlen("enable")) == 0) {
++		bpm_start();
++		return n;
++	}
++
++	if (n >= strlen("disable") &&
++	    strncmp(buf, "disable", strlen("disable")) == 0) {
++		bpm_stop();
++		return n;
++	}
++
++        printk(KERN_ERR "invalid input, please try \"enable\" or \"disable\"\n");
++	return n;
++}
++
++SYSDEV_ATTR(bpm, 0644, bpm_show, bpm_store);
++
++static ssize_t blink_show(struct sys_device *sys_dev, char *buf)
++{
++	char *s = buf;
++
++	if (g_dvfm_blink) 
++		s += sprintf(s, "blink: %s\n", g_dvfm_binfo.name);
++	else
++		s += sprintf(s, "blink: no\n");
++
++	return (s - buf);
++}
++
++static ssize_t blink_store(struct sys_device *sys_dev, const char *buf, size_t len)
++{
++	struct dvfm_blink_info binfo;
++
++	if (len >= (DVFM_BLINK_OWNER_LEN - 1)) {
++		printk(KERN_ERR "%s sets an invalid parameter of blink\n", current->comm);
++		return len;
++	}
++
++	memset(binfo.name, 0, sizeof(binfo.name));
++	sscanf(buf, "%s %d %*s", binfo.name, &binfo.time);
++
++	DPRINTK("blink: %s %d\n", binfo.name, binfo.time);
++
++	if (binfo.time < 0 || binfo.time > 3000) {
++		printk("%s sets an invalid time of blink\n", current->comm);
++		return len;
++	}
++
++	bpm_event_notify(IPM_EVENT_BLINK, IPM_EVENT_BLINK_SPEEDUP, &binfo, 
++			sizeof(struct dvfm_blink_info));
++
++	return len;
++}
++SYSDEV_ATTR(blink, 0666, blink_show, blink_store);
++
++static ssize_t log_show(struct sys_device *sys_dev, char *buf)
++{
++        char *s = buf;
++
++        s += sprintf(s, "%d\n", g_bpm_log_level);
++
++        return (s - buf);
++}
++
++static ssize_t log_store(struct sys_device *sys_dev, const char *buf, size_t n)
++{
++        sscanf(buf, "%u", &g_bpm_log_level);
++
++        if (g_bpm_log_level < 0 || g_bpm_log_level > 7) {
++                g_bpm_log_level = 0;
++                printk(KERN_ERR "invalid command\n");
++        }
++        return n;
++}
++
++SYSDEV_ATTR(log, 0644, log_show, log_store);
++
++static ssize_t cons_show(struct sys_device *sys_dev, char *buf)
++{
++        char *s = buf;
++        struct list_head *list = NULL;
++        struct bpm_cons *p = NULL;
++	int i;
++	unsigned long avg_ms;
++
++	for (i = 0; i < BPM_MAX_OP_NUM; ++i) {
++                s += sprintf(s, "op %d: %d\n", i, g_dyn_ops[i].count);
++                list_for_each(list, &(g_bpm_cons[i].list)) {
++			p = list_entry(list, struct bpm_cons, list);
++			if (p->tm)
++				avg_ms = p->ms / p->tm; 
++			else
++				avg_ms = 0;
++			s += sprintf(s, "\t%8ld %12ld %8ld %s: %d\n", 
++				p->tm, p->ms, avg_ms, p->sid, p->count);
++		}
++	} 
++
++        return (s - buf);
++}
++
++static ssize_t cons_store(struct sys_device *sys_dev, const char *buf, size_t n)
++{
++	struct list_head *list = NULL;
++	struct bpm_cons *p = NULL;
++	int i;
++	int cons_ctl = 0;
++
++	sscanf(buf, "%u", &cons_ctl);
++
++	if (1 == cons_ctl) {
++		for (i = 0; i < BPM_MAX_OP_NUM; ++i) {
++			list_for_each(list, &(g_bpm_cons[i].list)) {
++				p = list_entry(list, struct bpm_cons, list);
++				p->tm = 0;
++				p->ms = 0;
++				p->tmp_ms = 0;
++			}
++		}
++	}
++
++        return n;
++}
++
++SYSDEV_ATTR(cons, 0644, cons_show, cons_store);
++
++/*
++ * Dump blocked device on specified OP.
++ * And dump the device list that is tracked.
++ */
++static ssize_t trace_show(struct sys_device *sys_dev, char *buf)
++{
++	struct op_info *op_entry = NULL;
++	struct dvfm_trace_info *entry = NULL;
++	int len = 0, i;
++	unsigned int blocked_dev;
++
++	for (i = 0; i < op_nums; i++) {
++		blocked_dev = 0;
++		read_lock(&dvfm_op_list->lock);
++		/* op list shouldn't be empty because op_nums is valid */
++		list_for_each_entry(op_entry, &dvfm_op_list->list, list) {
++			if (op_entry->index == i)
++				blocked_dev = op_entry->device;
++		}
++		read_unlock(&dvfm_op_list->lock);
++		if (!blocked_dev)
++			continue;
++
++		len += sprintf(buf + len, "Blocked devices on OP%d:", i);
++		read_lock(&dvfm_trace_list.lock);
++		list_for_each_entry(entry, &dvfm_trace_list.list, list) {
++			if (test_bit(entry->index, (void *)&blocked_dev))
++				len += sprintf(buf + len, "%s, ", entry->name);
++		}
++		read_unlock(&dvfm_trace_list.lock);
++		len += sprintf(buf + len, "\n");
++	}
++	if (len == 0)
++		len += sprintf(buf + len, "None device block OP\n");
++	len += sprintf(buf + len, "Trace device list:\n");
++	read_lock(&dvfm_trace_list.lock);
++	list_for_each_entry(entry, &dvfm_trace_list.list, list) {
++		len += sprintf(buf + len, "%s, ", entry->name);
++	}
++	read_unlock(&dvfm_trace_list.lock);
++	len += sprintf(buf + len, "\n");
++	return len;
++}
++SYSDEV_ATTR(trace, 0444, trace_show, NULL);
++
++static struct attribute *bpm_attr[] = {
++	&attr_bpm.attr,
++	&attr_profiler_window.attr,
++	&attr_op.attr,
++	&attr_ops.attr,
++	&attr_enable_op.attr,
++	&attr_log.attr,
++	&attr_cons.attr,
++	&attr_blink.attr,
++	&attr_trace.attr,
++};
++
++static int bpm_add(struct sys_device *sys_dev)
++{
++	int i, n, ret;
++	n = ARRAY_SIZE(bpm_attr);
++	for (i = 0; i < n; ++i) {
++		ret = sysfs_create_file(&(sys_dev->kobj), bpm_attr[i]);
++		if (ret)
++			return ret;
++	}
++	return 0;
++}
++
++static int bpm_rm(struct sys_device *sys_dev)
++{
++	int i, n;
++	n = ARRAY_SIZE(bpm_attr);
++	for (i = 0; i < n; i++) {
++		sysfs_remove_file(&(sys_dev->kobj), bpm_attr[i]);
++	}
++	return 0;
++}
++
++static struct sysdev_driver bpm_driver = {
++	.add = bpm_add,
++	.remove = bpm_rm,
++};
++
++#ifdef CONFIG_TEST_BPMD
++#include "test_bpm.c"
++#endif
++/*****************************************************************************/
++/*                                                                           */
++/*                        BPMD Init & Fini                                   */
++/*                                                                           */
++/*****************************************************************************/
++
++static int __init bpm_init(void)
++{
++	unsigned int ret = 0;
++	unsigned long flag;
++
++	bpmq_init();
++
++	spin_lock_irqsave(&g_bpm_event_queue_lock, flag);
++
++	build_dyn_ops();
++	build_active_ops();
++
++	spin_unlock_irqrestore(&g_bpm_event_queue_lock, flag);
++
++	g_bpm_enabled = 0;
++	init_waitqueue_head(&g_bpm_enabled_waitq);
++
++	ret = sysdev_driver_register(&cpu_sysdev_class, &bpm_driver);
++	if (ret) {
++		printk(KERN_ERR "Can't register bpm sys driver,err:%d\n", ret);
++		PM_BUG_ON(1);
++	}
++
++#ifdef CONFIG_TEST_BPMD
++	ret = sysdev_driver_register(&cpu_sysdev_class, &bpm_test_driver);
++	if (ret) {
++		printk(KERN_ERR "Can't register bpm test driver,err:%d\n", ret);
++		PM_BUG_ON(1);
++	}
++#endif
++
++	dvfm_register("user-echo", &dvfm_dev_idx);
++
++#ifdef CONFIG_ANDROID_POWER
++	android_register_early_suspend(&bpm_early_suspend);
++#endif
++	init_timer(&g_dvfm_blink_timer);
++	g_dvfm_blink_timer.function = dvfm_blink_timer_handler;
++	g_dvfm_blink_timer.data = (unsigned long)NULL;
++
++	g_bpm_thread_exit = 0;
++	init_completion(&g_bpm_thread_over);
++	ret = kernel_thread(bpm_thread, NULL, 0);
++
++	printk(KERN_NOTICE "bpm init finished (%d)\n", ret);
++	return 0;
++}
++
++static void __exit bpm_exit(void)
++{
++
++	g_bpm_thread_exit = 1;
++
++#ifdef CONFIG_ANDROID_POWER	
++	android_unregister_early_suspend(&bpm_early_suspend);
++#endif
++	dvfm_unregister("user-echo", &dvfm_dev_idx);
++
++	g_bpm_enabled = 1;
++	wake_up_interruptible(&g_bpm_enabled_waitq);
++	wake_up_interruptible(&g_bpm_event_queue.waitq);
++	wait_for_completion(&g_bpm_thread_over);
++	g_bpm_enabled = 0;
++}
++
++module_init(bpm_init);
++module_exit(bpm_exit);
++
++MODULE_DESCRIPTION("BPMD");
++MODULE_LICENSE("GPL");
+diff -ur linux-2.6.32/arch/arm/mach-pxa/bpm_prof.c kernel/arch/arm/mach-pxa/bpm_prof.c
+--- linux-2.6.32/arch/arm/mach-pxa/bpm_prof.c	2009-12-13 12:58:12.232379200 +0200
++++ kernel/arch/arm/mach-pxa/bpm_prof.c	2009-12-12 16:09:26.429614458 +0200
+@@ -0,0 +1,564 @@
++/*
++ * PXA3xx IPM Profiler
++ *
++ * Copyright (C) 2008 Borqs Ltd.
++ * Emichael Li <emichael.li@borqs.com>
++ *
++ * Based on Marvell v6.5 release.
++ *
++ * Copyright (C) 2008 Marvell Corporation
++ * Haojian Zhuang <haojian.zhuang@marvell.com>
++ *
++ * This software program is licensed subject to the GNU General Public License
++ * (GPL).Version 2,June 1991, available at http://www.fsf.org/copyleft/gpl.html
++
++ * (C) Copyright 2008 Marvell International Ltd.
++ * All Rights Reserved
++ */
++
++#include <linux/init.h>
++#include <linux/module.h>
++#include <linux/errno.h>
++#include <linux/sched.h>
++#include <linux/tick.h>
++#include <linux/timer.h>
++#include <linux/device.h>
++#include <linux/jiffies.h>
++#include <mach/hardware.h>
++#include <mach/mspm_prof.h>
++#include <asm/arch/ipmc.h>
++#ifdef CONFIG_PXA3xx_DVFM
++#include <asm/arch/dvfm.h>
++#include <asm/arch/pxa3xx_dvfm.h>
++#endif
++
++extern int (*pipm_start_pmu)(struct ipm_profiler_arg *arg);
++extern int (*pipm_stop_pmu)(void);
++
++/* IDLE profiler tune OP with MIPS feature */
++#define MSPM_IDLE_PROF_MIPS	0	
++
++#undef MAX_OP_NUM
++#define MAX_OP_NUM	10
++
++struct mspm_op_stats {
++	int		op;
++	int		idle;
++	unsigned int	timestamp;
++	unsigned int 	jiffies;
++};
++
++struct mspm_mips {
++	int	mips;
++	int	h_thres;	/* high threshold */
++	int	l_thres;	/* low threshold */
++};
++
++/* Store costed time in run_op_time[] & idle_op_time[] */
++static int run_op_time[MAX_OP_NUM], idle_op_time[MAX_OP_NUM];
++
++/*
++ * Store OP's MIPS in op_mips[].
++ * The lowest frequency OP is the first entry.
++ */
++static struct mspm_mips op_mips[MAX_OP_NUM];
++
++/* Store the calculated MIPS of last sample window */
++static int last_mips;
++
++/*
++ * Store the first timestamp of sample window in first_stats
++ * Store the current timestamp of sample window in cur_stats
++ */
++static struct mspm_op_stats first_stats, cur_stats;
++
++/* OP numbers used in IPM IDLE Profiler */
++static int mspm_op_num = 0;
++
++static struct timer_list idle_prof_timer;
++
++/* PMU result is stored in it */
++static struct pmu_results sum_pmu_res;
++
++static int mspm_prof_enabled = 0;
++static int window_jif = 0;
++static int mspm_pmu_id;
++
++unsigned int prof_idle_time, prof_time;
++
++static int mspm_prof_notifier_freq(struct notifier_block *nb,
++				unsigned long val, void *data);
++static struct notifier_block notifier_freq_block = {
++	.notifier_call = mspm_prof_notifier_freq,
++};
++
++static unsigned int read_time(void)
++{
++#ifdef CONFIG_PXA_32KTIMER
++	return OSCR4;
++#else
++	return OSCR0;
++#endif
++}
++
++
++static int bpm_mod_timer(struct timer_list *timer, unsigned long expires)
++{
++#ifdef CONFIG_BPMD
++	extern void timer_set_deferrable(struct timer_list *timer);
++	extern void timer_clr_deferrable(struct timer_list *timer);
++	extern int get_op_power_bonus(void);
++
++	if (get_op_power_bonus())
++		timer_set_deferrable(timer);
++	else 
++		timer_clr_deferrable(timer);
++#endif	
++	mod_timer(timer, expires);
++
++	return 0;
++}
++
++/*
++ * Record the OP index and RUN/IDLE state.
++ */
++int mspm_add_event(int op, int cpu_idle)
++{
++	unsigned int time;
++
++	if (mspm_prof_enabled) {
++		time = read_time();
++		/* sum the current sample window */
++		if (cpu_idle == CPU_STATE_IDLE)
++			idle_op_time[cur_stats.op] +=
++				time - cur_stats.timestamp;
++		else if (cpu_idle == CPU_STATE_RUN)
++			run_op_time[cur_stats.op] +=
++				time - cur_stats.timestamp;
++		/* update start point of current sample window */
++		cur_stats.op = op;
++		cur_stats.idle = cpu_idle;
++		cur_stats.timestamp = time;
++		cur_stats.jiffies = jiffies;
++	}
++	return 0;
++}
++EXPORT_SYMBOL(mspm_add_event);
++
++/*
++ * Prepare to do a new sample.
++ * Clear the index in mspm_op_stats table.
++ */
++static int mspm_do_new_sample(void)
++{
++	/* clear previous sample window */
++	memset(&run_op_time, 0, sizeof(int) * MAX_OP_NUM);
++	memset(&idle_op_time, 0, sizeof(int) * MAX_OP_NUM);
++	/* prepare for the new sample window */
++	first_stats.op = cur_stats.op;
++	first_stats.idle = cur_stats.idle;
++	first_stats.timestamp = read_time();
++	first_stats.jiffies = jiffies;
++
++	prof_idle_time = 0;
++	prof_time = read_time();
++	return 0;
++}
++
++/*
++ * Init MIPS of all OP
++ */
++static int mspm_init_mips(void)
++{
++	struct op_info *info = NULL;
++	struct dvfm_md_opt *md_op = NULL;
++	int i, ret;
++	memset(&op_mips, 0, MAX_OP_NUM * sizeof(struct mspm_mips));
++	mspm_op_num = dvfm_op_count();
++#ifdef CONFIG_PXA3xx_DVFM
++	for (i = 0; i < mspm_op_num; i++) {
++		ret = dvfm_get_opinfo(i, &info);
++		if (ret)
++			continue;
++		md_op = (struct dvfm_md_opt *)info->op;
++		op_mips[i].mips = md_op->core;
++		if (op_mips[i].mips) {
++			op_mips[i].h_thres = DEF_HIGH_THRESHOLD;
++			if (!strcmp(md_op->name, "D0CS"))
++				op_mips[i].h_thres = 95;
++		} else {
++			mspm_op_num = i;
++			break;
++		}
++	}
++	for (i = 0; i < mspm_op_num - 1; i++)
++		op_mips[i + 1].l_thres = op_mips[i].h_thres * op_mips[i].mips
++				/ op_mips[i + 1].mips;
++#endif
++	return 0;
++}
++
++/*
++ * Calculate the MIPS in sample window
++ */
++static int mspm_calc_mips(void)
++{
++	int i;
++	unsigned int sum_time = 0, sum = 0;
++
++	/* Calculate total time costed in sample window */
++	for (i = 0; i < mspm_op_num; i++) {
++		sum_time += run_op_time[i] + idle_op_time[i];
++		sum += run_op_time[i] * op_mips[i].mips;
++	}
++	if (sum_time == 0)
++		return 0;
++
++	/*
++	 * Calculate MIPS in sample window
++	 * Formula: run_op_time[i] / sum_time * op_mips[i].mips
++	 */
++	return (sum / sum_time);
++}
++
++static int is_valid_sample_window(void)
++{
++	unsigned int time;
++	/* The sample window isn't started */
++	if (!mspm_prof_enabled)
++		goto out;
++	time = cur_stats.jiffies - first_stats.jiffies;
++	time = jiffies_to_msecs(time);
++	if (time >= MIN_SAMPLE_WINDOW)
++		return 1;
++out:
++	return 0;
++}
++
++/*
++ * When DVFM release one OP, it will invoke this func to get the prefered OP.
++ */
++static int mspm_get_mips(void)
++{
++	int ret;
++	extern int cur_op;
++
++	mspm_add_event(cur_op, CPU_STATE_RUN);
++
++	if (!is_valid_sample_window()) {
++		/* This sample window is invalide, use MIPS value of last
++		 * sample window
++		 */
++		ret = last_mips;
++		goto out_sample;
++	}
++	ret = mspm_calc_mips();
++	if (ret < 0)
++		goto out_calc;
++	return ret;
++out_calc:
++	printk(KERN_WARNING "Can't calculate MIPS\n");
++out_sample:
++	return ret;
++}
++
++/*
++ * Adjust to the most appropriate OP according to MIPS result of
++ * sample window
++ */
++#if MSPM_IDLE_PROF_MIPS
++int mspm_tune(void)
++{
++	int i, mips;
++	if (mspm_prof_enabled) {
++		for (i = mspm_op_num - 1; i >= 0; i--) {
++			mips = mspm_get_mips();
++			if (mips >= (op_mips[i].l_thres *
++				op_mips[i].mips / 100))
++				break;
++		}
++		dvfm_request_op(i);
++	}
++	return 0;
++}
++#else
++int mspm_tune(void) { return 0; }
++#endif
++EXPORT_SYMBOL(mspm_tune);
++
++/***************************************************************************
++ * 			Idle Profiler
++ ***************************************************************************
++ */
++
++static struct ipm_profiler_arg pmu_arg;
++static int mspm_start_prof(struct ipm_profiler_arg *arg)
++{
++	struct pmu_results res;
++	struct op_info *info = NULL;
++
++	memset(&sum_pmu_res, 0, sizeof(struct pmu_results));
++
++	/* pmu_arg.window_size stores the number of miliseconds.
++	 * window_jif stores the number of jiffies.
++	 */
++	memset(&pmu_arg, 0, sizeof(struct ipm_profiler_arg));
++	pmu_arg.flags = arg->flags;
++	if (arg->window_size > 0)
++		pmu_arg.window_size = arg->window_size;
++	else
++		pmu_arg.window_size = DEF_SAMPLE_WINDOW;
++	window_jif = msecs_to_jiffies(pmu_arg.window_size);
++	if ((mspm_pmu_id > 0) && (pmu_arg.flags & IPM_PMU_PROFILER)) {
++		pmu_arg.pmn0 = arg->pmn0;
++		pmu_arg.pmn1 = arg->pmn1;
++		pmu_arg.pmn2 = arg->pmn2;
++		pmu_arg.pmn3 = arg->pmn3;
++		/* Collect PMU information */
++		if (pmu_stop(&res))
++			printk(KERN_WARNING
++				"L:%d: pmu_stop failed!\n", __LINE__);
++		if (pmu_start(pmu_arg.pmn0, pmu_arg.pmn1, pmu_arg.pmn2,
++				pmu_arg.pmn3))
++			printk(KERN_WARNING
++				"L:%d: pmu_start failed!\n", __LINE__);
++	}
++	/* start next sample window */
++	cur_stats.op = dvfm_get_op(&info);
++	cur_stats.idle = CPU_STATE_RUN;
++	cur_stats.timestamp = read_time();
++	cur_stats.jiffies = jiffies;
++	mspm_do_new_sample();
++	bpm_mod_timer(&idle_prof_timer, jiffies + window_jif);
++	mspm_prof_enabled = 1;
++	return 0;
++}
++
++static int mspm_stop_prof(void)
++{
++	struct pmu_results res;
++	if ((mspm_pmu_id > 0) && (pmu_arg.flags & IPM_PMU_PROFILER)) {
++		if (pmu_stop(&res))
++			printk(KERN_WARNING
++				"L:%d: pmu_stop failed!\n", __LINE__);
++	}
++	del_timer(&idle_prof_timer);
++	mspm_prof_enabled = 0;
++	return 0;
++}
++
++static int calc_pmu_res(struct pmu_results *res)
++{
++	if (res == NULL)
++		return -EINVAL;
++	sum_pmu_res.ccnt += res->ccnt;
++	sum_pmu_res.pmn0 += res->pmn0;
++	sum_pmu_res.pmn1 += res->pmn1;
++	sum_pmu_res.pmn2 += res->pmn2;
++	sum_pmu_res.pmn3 += res->pmn3;
++	return 0;
++}
++
++/*
++ * Pause idle profiler when system enter Low Power mode.
++ * Continue it when system exit from Low Power mode.
++ */
++void set_idletimer(int enable)
++{
++	struct pmu_results res;
++	if (enable && mspm_prof_enabled) {
++		/*
++		 * Restart the idle profiler because it's only disabled
++		 * before entering low power mode.
++		 * If we just continue the sample window with left jiffies,
++		 * too much OS Timer wakeup exist in system.
++		 * Just restart the sample window.
++		 */ 
++		bpm_mod_timer(&idle_prof_timer, jiffies + window_jif);
++		tick_nohz_restart_sched_tick();
++
++		first_stats.jiffies = jiffies;
++		first_stats.timestamp = read_time();
++
++		if (pmu_arg.flags & IPM_PMU_PROFILER) {
++			if (pmu_start(pmu_arg.pmn0, pmu_arg.pmn1, pmu_arg.pmn2,
++						pmu_arg.pmn3)) {
++				printk(KERN_WARNING
++						"L:%d: pmu_start failed!\n", __LINE__);
++			}
++		}
++	} else if (!enable && mspm_prof_enabled) {
++		del_timer(&idle_prof_timer);
++		tick_nohz_stop_sched_tick(1);
++
++		if (pmu_arg.flags & IPM_PMU_PROFILER) {
++			if (pmu_stop(&res)) {
++				printk(KERN_WARNING
++						"L:%d: pmu_stop failed!\n", __LINE__);
++			} else
++				calc_pmu_res(&res);
++		}
++	}
++}
++EXPORT_SYMBOL(set_idletimer);
++
++/*
++ * Handler of IDLE PROFILER
++ */
++static void idle_prof_handler(unsigned long data)
++{
++	struct ipm_profiler_result out_res;
++	struct pmu_results res;
++	struct op_info *info = NULL;
++	int ret, mips, op;
++
++	if (!mspm_prof_enabled)
++		return;
++
++	ret = mspm_get_mips();
++	if (ret >= 0)
++		mips = ret;
++	else
++		mips = last_mips;
++	if ((mspm_pmu_id > 0) && (pmu_arg.flags & IPM_PMU_PROFILER)) {
++		if (pmu_stop(&res))
++			printk(KERN_WARNING "pmu_stop failed %d\n", __LINE__);
++		else
++			calc_pmu_res(&res);
++		if (pmu_start(pmu_arg.pmn0, pmu_arg.pmn1, pmu_arg.pmn2,
++				pmu_arg.pmn3))
++			printk(KERN_WARNING "pmu_start failed %d\n", __LINE__);
++		memset(&out_res, 0, sizeof(struct ipm_profiler_result));
++		out_res.pmu.ccnt = sum_pmu_res.ccnt;
++		out_res.pmu.pmn0 = sum_pmu_res.pmn0;
++		out_res.pmu.pmn1 = sum_pmu_res.pmn1;
++		out_res.pmu.pmn2 = sum_pmu_res.pmn2;
++		out_res.pmu.pmn3 = sum_pmu_res.pmn3;
++	}
++	op = dvfm_get_op(&info);
++
++#if 0	
++	/* When system is running, MIPS of current OP won't be zero. */
++	out_res.busy_ratio = mips * 100 / op_mips[op].mips;
++	out_res.window_size = jiffies_to_msecs(window_jif);
++#endif
++
++	prof_time = read_time() - prof_time;
++
++	out_res.busy_ratio = 100 - 100 * prof_idle_time / prof_time;
++	out_res.window_size = 0;	/* not used */
++	out_res.mips = mips;
++
++	/* send PMU result to policy maker in user space */
++	bpm_event_notify(IPM_EVENT_PROFILER, pmu_arg.flags, &out_res,
++			sizeof(struct ipm_profiler_result));
++
++#if 0
++	/* start next sample window */
++	mspm_do_new_sample();
++	bpm_mod_timer(&idle_prof_timer, jiffies + window_jif);
++	memset(&sum_pmu_res, 0, sizeof(struct pmu_results));
++#endif
++	last_mips = mips;
++}
++
++/*
++ * Pause idle profiler when system enter Low Power mode.
++ * Continue it when system exit from Low Power mode.
++ */
++static int mspm_prof_notifier_freq(struct notifier_block *nb,
++				unsigned long val, void *data)
++{
++	struct dvfm_freqs *freqs = (struct dvfm_freqs *)data;
++	struct op_info *info = &(freqs->new_info);
++	struct dvfm_md_opt *md = NULL;
++	struct pmu_results res;
++
++	if (!mspm_prof_enabled)
++		return 0;
++	md = (struct dvfm_md_opt *)(info->op);
++	if (md->power_mode == POWER_MODE_D1 ||
++		md->power_mode == POWER_MODE_D2 ||
++		md->power_mode == POWER_MODE_CG) {
++		switch (val) {
++		case DVFM_FREQ_PRECHANGE:
++			del_timer(&idle_prof_timer);
++			tick_nohz_stop_sched_tick(1);
++			if (pmu_arg.flags & IPM_PMU_PROFILER) {
++				if (pmu_stop(&res))
++					printk(KERN_WARNING
++						"L:%d: pmu_stop failed!\n",
++						__LINE__);
++				else
++					calc_pmu_res(&res);
++			}
++			break;
++		case DVFM_FREQ_POSTCHANGE:
++			/* Update jiffies and touch watchdog process */
++			tick_nohz_update_jiffies();
++			/*
++			 * Restart the idle profiler because it's only
++			 * disabled before entering low power mode.
++			 * If we just continue the sample window with
++			 * left jiffies, too much OS Timer wakeup exist
++			 * in system.
++			 * Just restart the sample window.
++			 */ 
++			bpm_mod_timer(&idle_prof_timer, jiffies + window_jif);
++			first_stats.jiffies = jiffies;
++			first_stats.timestamp = read_time();
++		
++			if (pmu_arg.flags & IPM_PMU_PROFILER)
++				if (pmu_start(pmu_arg.pmn0, pmu_arg.pmn1,
++					pmu_arg.pmn2, pmu_arg.pmn3))
++					printk(KERN_WARNING
++						"L:%d: pmu_start failed!\n",
++						__LINE__);
++			break;
++		}
++	}
++	return 0;
++}
++
++int __init mspm_prof_init(void)
++{
++	mspm_pmu_id = pmu_claim();
++
++	memset(&pmu_arg, 0, sizeof(struct ipm_profiler_arg));
++	pmu_arg.window_size = DEF_SAMPLE_WINDOW;
++	pmu_arg.pmn0 = PMU_EVENT_POWER_SAVING;
++	pmu_arg.pmn1 = PMU_EVENT_POWER_SAVING;
++	pmu_arg.pmn2 = PMU_EVENT_POWER_SAVING;
++	pmu_arg.pmn3 = PMU_EVENT_POWER_SAVING;
++	window_jif = msecs_to_jiffies(pmu_arg.window_size);
++
++	pipm_start_pmu = mspm_start_prof;
++	pipm_stop_pmu = mspm_stop_prof;
++
++	/* It's used to trigger sample window.
++	 * If system is idle, the timer could be deferred.
++	 */
++	init_timer(&idle_prof_timer);
++	idle_prof_timer.function = idle_prof_handler;
++	idle_prof_timer.data = 0;
++
++	mspm_init_mips();
++
++	dvfm_register_notifier(&notifier_freq_block,
++				DVFM_FREQUENCY_NOTIFIER);
++
++	return 0;
++}
++
++void __exit mspm_prof_exit(void)
++{
++	dvfm_unregister_notifier(&notifier_freq_block,
++				DVFM_FREQUENCY_NOTIFIER);
++
++	if (mspm_pmu_id)
++		pmu_release(mspm_pmu_id);
++
++	pipm_start_pmu = NULL;
++	pipm_stop_pmu = NULL;
++}
++
+diff -ur linux-2.6.32/arch/arm/mach-pxa/devices.c kernel/arch/arm/mach-pxa/devices.c
+--- linux-2.6.32/arch/arm/mach-pxa/devices.c	2009-12-03 05:51:21.000000000 +0200
++++ kernel/arch/arm/mach-pxa/devices.c	2009-12-12 16:09:26.436277478 +0200
+@@ -15,6 +15,7 @@
+ #include <mach/camera.h>
+ #include <mach/audio.h>
+ #include <mach/pxa3xx_nand.h>
++#include <mach/pxa3xx_dvfm.h>
+ 
+ #include "devices.h"
+ #include "generic.h"
+@@ -962,6 +963,76 @@
+ 	},
+ };
+ 
++static struct resource pxa3xx_resource_freq[] = {
++	[0] = {
++		.name   = "clkmgr_regs",
++		.start	= 0x41340000,
++		.end	= 0x41350003,
++		.flags	= IORESOURCE_MEM,
++	},
++	[1] = {
++		.name   = "spmu_regs",
++		.start	= 0x40f50000,
++		.end	= 0x40f50103,
++		.flags	= IORESOURCE_MEM,
++	},
++	[2] = {
++		.name   = "bpmu_regs",
++		.start	= 0x40f40000,
++		.end	= 0x40f4003b,
++		.flags	= IORESOURCE_MEM,
++	},
++	[3] = {
++		.name   = "dmc_regs",
++		.start	= 0x48100000,
++		.end	= 0x4810012f,
++		.flags	= IORESOURCE_MEM,
++	},
++	[4] = {
++		.name   = "smc_regs",
++		.start	= 0x4a000000,
++		.end	= 0x4a00008f,
++		.flags	= IORESOURCE_MEM,
++	}
++};
++
++struct platform_device pxa3xx_device_freq = {
++	.name		= "pxa3xx-freq",
++	.id		= 0,
++	.num_resources	= ARRAY_SIZE(pxa3xx_resource_freq),
++	.resource	= pxa3xx_resource_freq,
++};
++
++void __init set_pxa3xx_freq_info(struct pxa3xx_freq_mach_info *info)
++{
++	pxa_register_device(&pxa3xx_device_freq, info);
++}
++
++void __init set_pxa3xx_freq_parent(struct device *parent_dev)
++{
++	pxa3xx_device_freq.dev.parent = parent_dev;
++} 
++
++static struct resource pxa3xx_pmu_resources[] = {
++	[0] = {
++		.name   = "pmu_regs",
++		.start = 0x4600ff00,
++		.end   = 0x4600ffff,
++		.flags = IORESOURCE_MEM,
++	},
++};
++
++struct platform_device pxa3xx_device_pmu = {
++	.name		= "pxa3xx-pmu",
++	.id		= 0,
++	.resource       = pxa3xx_pmu_resources,
++	.num_resources  = ARRAY_SIZE(pxa3xx_pmu_resources),
++};
++
++void __init pxa3xx_set_pmu_info(void *info)
++{
++	pxa_register_device(&pxa3xx_device_pmu, info);
++} 
+ #endif /* CONFIG_PXA3xx */
+ 
+ /* pxa2xx-spi platform-device ID equals respective SSP platform-device ID + 1.
+diff -ur linux-2.6.32/arch/arm/mach-pxa/devices.h kernel/arch/arm/mach-pxa/devices.h
+--- linux-2.6.32/arch/arm/mach-pxa/devices.h	2009-12-03 05:51:21.000000000 +0200
++++ kernel/arch/arm/mach-pxa/devices.h	2009-12-12 16:09:26.436277478 +0200
+@@ -36,5 +36,6 @@
+ extern struct platform_device pxa3xx_device_i2c_power;
+ 
+ extern struct platform_device pxa3xx_device_gcu;
++extern struct platform_device pxa3xx_device_freq;
+ 
+ void __init pxa_register_device(struct platform_device *dev, void *data);
+diff -ur linux-2.6.32/arch/arm/mach-pxa/dvfm.c kernel/arch/arm/mach-pxa/dvfm.c
+--- linux-2.6.32/arch/arm/mach-pxa/dvfm.c	2009-12-13 12:58:54.725287534 +0200
++++ kernel/arch/arm/mach-pxa/dvfm.c	2009-12-12 16:09:26.439612372 +0200
+@@ -0,0 +1,922 @@
++/*
++ * DVFM Abstract Layer
++ *
++ * This software program is licensed subject to the GNU General Public License
++ * (GPL).Version 2,June 1991, available at http://www.fsf.org/copyleft/gpl.html
++
++ * (C) Copyright 2007 Marvell International Ltd.
++ * All Rights Reserved
++ */
++
++#include <linux/init.h>
++#include <linux/kernel.h>
++#include <linux/module.h>
++#include <linux/device.h>
++#include <linux/sysdev.h>
++#include <linux/spinlock.h>
++#include <linux/notifier.h>
++#include <linux/string.h>
++#include <linux/kobject.h>
++#include <linux/list.h>
++#include <linux/notifier.h>
++#include <asm/atomic.h>
++#include <mach/dvfm.h>
++
++#ifdef CONFIG_BPMD
++#include <mach/bpm.h>
++
++extern int bpm_enable_op(int index, int dev_idx);
++extern int bpm_disable_op(int index, int dev_idx);
++extern int bpm_enable_op_name(char *name, int dev_idx, char *sid);
++extern int bpm_disable_op_name(char *name, int dev_idx, char *sid);
++#endif
++
++#define MAX_DEVNAME_LEN	32
++/* This structure is used to dump device name list */
++struct name_list {
++	int	id;
++	char	name[MAX_DEVNAME_LEN];
++};
++
++static ATOMIC_NOTIFIER_HEAD(dvfm_freq_notifier_list);
++
++/* This list links log of dvfm operation */
++struct info_head dvfm_trace_list = {
++	.list = LIST_HEAD_INIT(dvfm_trace_list.list),
++	.lock = RW_LOCK_UNLOCKED,
++	.device = 0,
++};
++
++#ifndef CONFIG_BPMD
++/* This idx is used for user debug */
++static int dvfm_dev_idx;
++#endif
++
++struct dvfm_driver *dvfm_driver = NULL;
++struct info_head *dvfm_op_list = NULL;
++
++unsigned int cur_op;			/* current operating point */
++unsigned int def_op;			/* default operating point */
++unsigned int op_nums = 0;		/* number of operating point */
++
++static atomic_t lp_count = ATOMIC_INIT(0);	/* number of blocking lowpower mode */
++
++extern struct sysdev_class cpu_sysdev_class;
++
++int dvfm_find_op(int index, struct op_info **op)
++{
++	struct op_info *p = NULL;
++
++	read_lock(&dvfm_op_list->lock);
++	if (list_empty(&dvfm_op_list->list)) {
++		read_unlock(&dvfm_op_list->lock);
++		return -ENOENT;
++	}
++	list_for_each_entry(p, &dvfm_op_list->list, list) {
++		if (p->index == index) {
++			*op = p;
++			read_unlock(&dvfm_op_list->lock);
++			return 0;
++		}
++	}
++	read_unlock(&dvfm_op_list->lock);
++	return -ENOENT;
++}
++
++#ifndef CONFIG_BPMD
++/* Display current operating point */
++static ssize_t op_show(struct sys_device *sys_dev, struct sysdev_attribute *attr,char *buf)
++{
++	struct op_info *op = NULL;
++	int len = 0;
++
++	if (dvfm_driver->dump) {
++		if (!dvfm_find_op(cur_op, &op)) {
++			len = dvfm_driver->dump(dvfm_driver->priv, op, buf);
++		}
++	}
++
++	return len;
++}
++
++/* Set current operating point */
++static ssize_t op_store(struct sys_device *sys_dev, struct sysdev_attribute *attr, const char *buf,
++				size_t len)
++{
++	struct dvfm_freqs freqs;
++	int new_op;
++
++	sscanf(buf, "%u", &new_op);
++	dvfm_request_op(new_op);
++	return len;
++}
++SYSDEV_ATTR(op, 0644, op_show, op_store);
++
++/* Dump all operating point */
++static ssize_t ops_show(struct sys_device *sys_dev, struct sysdev_attribute *attr, char *buf)
++{
++	struct op_info *entry = NULL;
++	int len = 0;
++	char *p = NULL;
++
++	if (!dvfm_driver->dump)
++		return 0;
++	read_lock(&dvfm_op_list->lock);
++	if (!list_empty(&dvfm_op_list->list)) {
++		list_for_each_entry(entry, &dvfm_op_list->list, list) {
++			p = buf + len;
++			len += dvfm_driver->dump(dvfm_driver->priv, entry, p);
++		}
++	}
++	read_unlock(&dvfm_op_list->lock);
++
++	return len;
++}
++SYSDEV_ATTR(ops, 0444, ops_show, NULL);
++
++/* Dump all enabled operating point */
++static ssize_t enable_op_show(struct sys_device *sys_dev, struct sysdev_attribute *attr, char *buf)
++{
++	struct op_info *entry = NULL;
++	int len = 0;
++	char *p = NULL;
++
++	if (!dvfm_driver->dump)
++		return 0;
++	read_lock(&dvfm_op_list->lock);
++	if (!list_empty(&dvfm_op_list->list)) {
++		list_for_each_entry(entry, &dvfm_op_list->list, list) {
++			if (!entry->device) {
++				p = buf + len;
++				len += dvfm_driver->dump(dvfm_driver->priv, entry, p);
++			}
++		}
++	}
++	read_unlock(&dvfm_op_list->lock);
++
++	return len;
++}
++
++static ssize_t enable_op_store(struct sys_device *sys_dev, struct sysdev_attribute *attr, const char *buf,
++				size_t len)
++{
++	int op, level;
++
++	sscanf(buf, "%u,%u", &op, &level);
++	if (level) {
++		dvfm_enable_op(op, dvfm_dev_idx);
++	} else
++		dvfm_disable_op(op, dvfm_dev_idx);
++	return len;
++}
++SYSDEV_ATTR(enable_op, 0644, enable_op_show, enable_op_store);
++
++/*
++ * Dump blocked device on specified OP.
++ * And dump the device list that is tracked.
++ */
++static ssize_t trace_show(struct sys_device *sys_dev, struct sysdev_attribute *attr, char *buf)
++{
++	struct op_info *op_entry = NULL;
++	struct dvfm_trace_info *entry = NULL;
++	int len = 0, i;
++	unsigned int blocked_dev;
++
++	for (i = 0; i < op_nums; i++) {
++		blocked_dev = 0;
++		read_lock(&dvfm_op_list->lock);
++		/* op list shouldn't be empty because op_nums is valid */
++		list_for_each_entry(op_entry, &dvfm_op_list->list, list) {
++			if (op_entry->index == i)
++				blocked_dev = op_entry->device;
++		}
++		read_unlock(&dvfm_op_list->lock);
++		if (!blocked_dev)
++			continue;
++
++		len += sprintf(buf + len, "Blocked devices on OP%d:", i);
++		read_lock(&dvfm_trace_list.lock);
++		list_for_each_entry(entry, &dvfm_trace_list.list, list) {
++			if (test_bit(entry->index, (void *)&blocked_dev))
++				len += sprintf(buf + len, "%s, ", entry->name);
++		}
++		read_unlock(&dvfm_trace_list.lock);
++		len += sprintf(buf + len, "\n");
++	}
++	if (len == 0)
++		len += sprintf(buf + len, "None device block OP\n");
++	len += sprintf(buf + len, "Trace device list:\n");
++	read_lock(&dvfm_trace_list.lock);
++	list_for_each_entry(entry, &dvfm_trace_list.list, list) {
++		len += sprintf(buf + len, "%s, ", entry->name);
++	}
++	read_unlock(&dvfm_trace_list.lock);
++	len += sprintf(buf + len, "\n");
++	return len;
++}
++SYSDEV_ATTR(trace, 0444, trace_show, NULL);
++
++#ifdef CONFIG_CPU_PXA310
++static ssize_t freq_show(struct sys_device *sys_dev, struct sysdev_attribute *attr, char *buf)
++{
++	struct op_info *op = NULL;
++	int len = 0;
++
++	if (dvfm_driver->freq_show) {
++		if (!dvfm_find_op(cur_op, &op)) {
++			len = dvfm_driver->freq_show(dvfm_driver->priv, op, buf);
++		}
++	}
++
++	return len;
++}
++/* 
++ * We can define a freq_store to set frequencies with a lot of parameters,
++ * If a new set of frequencies is inputed by that way, it will only be treated 
++ * as a non-standard op, not a new op. So the freq_store function isn't defined.
++ */
++SYSDEV_ATTR(frequency, 0644, freq_show, NULL);
++#endif
++
++static struct attribute *dvfm_attr[] = {
++	&attr_op.attr,
++	&attr_ops.attr,
++	&attr_enable_op.attr,
++	&attr_trace.attr,
++#ifdef CONFIG_CPU_PXA310
++	&attr_frequency.attr,
++#endif
++};
++#endif
++
++int dvfm_op_count(void)
++{
++	int ret = -EINVAL;
++
++	if (dvfm_driver && dvfm_driver->count)
++		ret = dvfm_driver->count(dvfm_driver->priv, dvfm_op_list);
++	return ret;
++}
++EXPORT_SYMBOL(dvfm_op_count);
++
++int dvfm_get_op(struct op_info **p)
++{
++	if (dvfm_find_op(cur_op, p))
++		return -EINVAL;
++	return cur_op;
++}
++EXPORT_SYMBOL(dvfm_get_op);
++
++int dvfm_dump_op(int idx, char *buf)
++{
++        struct op_info *op = NULL;
++        int len = 0;
++
++        if (dvfm_driver && dvfm_driver->dump && !dvfm_find_op(idx, &op)) 
++        	len = dvfm_driver->dump(dvfm_driver->priv, op, buf);
++
++        return len;
++}
++EXPORT_SYMBOL(dvfm_dump_op);
++
++int dvfm_get_op_freq(int idx, struct op_freq *pf)
++{
++        struct op_info *op = NULL;
++        int ret = 0;
++
++        if (dvfm_driver && dvfm_driver->get_freq && !dvfm_find_op(idx, &op))
++                ret = dvfm_driver->get_freq(dvfm_driver->priv, op, pf);
++
++        return ret;
++}
++EXPORT_SYMBOL(dvfm_get_op_freq);
++
++int dvfm_check_active_op(int idx)
++{
++        struct op_info *op = NULL;
++        int ret = 0;
++
++        if (dvfm_driver && dvfm_driver->check_active_op && !dvfm_find_op(idx, &op))
++                ret = dvfm_driver->check_active_op(dvfm_driver->priv, op);
++
++        return ret;
++}
++EXPORT_SYMBOL(dvfm_check_active_op);
++
++int dvfm_get_defop(void)
++{
++	return def_op;
++}
++EXPORT_SYMBOL(dvfm_get_defop);
++
++int dvfm_get_opinfo(int index, struct op_info **p)
++{
++	if (dvfm_find_op(index, p))
++		return -EINVAL;
++	return 0;
++}
++EXPORT_SYMBOL(dvfm_get_opinfo);
++
++
++const char* dvfm_get_op_name(int idx)
++{
++        struct op_info *op = NULL;
++
++        if (dvfm_driver && dvfm_driver->name && !dvfm_find_op(idx, &op))
++		return dvfm_driver->name(dvfm_driver->priv, op);
++
++        return NULL;
++}
++EXPORT_SYMBOL(dvfm_get_op_name);
++
++
++int dvfm_set_op(struct dvfm_freqs *freqs, unsigned int new,
++		unsigned int relation)
++{
++	int ret = -EINVAL;
++
++	/* check whether dvfm is enabled */
++	if (!dvfm_driver || !dvfm_driver->count)
++		return -EINVAL;
++	if (dvfm_driver->set)
++		ret = dvfm_driver->set(dvfm_driver->priv, freqs, new, relation);
++	return ret;
++}
++
++/* Request operating point. System may set higher frequency because of
++ * device constraint.
++ */
++int dvfm_request_op(int index)
++{
++	int ret = -EFAULT;
++
++	/* check whether dvfm is enabled */
++	if (!dvfm_driver || !dvfm_driver->count)
++		return -EINVAL;
++#ifdef CONFIG_BPMD
++	printk(KERN_ERR "please don't use this API\n");
++	WARN_ON(1);
++#endif
++
++	if (dvfm_driver->request_set)
++		ret = dvfm_driver->request_set(dvfm_driver->priv, index);
++
++	return ret;
++}
++EXPORT_SYMBOL(dvfm_request_op);
++
++/*
++ * Device remove the constraint on OP.
++ */
++int __dvfm_enable_op(int index, int dev_idx)
++{
++	struct op_info *p = NULL;
++	int num;
++
++	/* check whether dvfm is enabled */
++	if (!dvfm_driver || !dvfm_driver->count)
++		return -EINVAL;
++	/* only registered device can invoke DVFM operation */
++	if ((dev_idx >= DVFM_MAX_DEVICE) || dev_idx < 0)
++		return -ENOENT;
++	num = dvfm_driver->count(dvfm_driver->priv, dvfm_op_list);
++	if (num <= index)
++		return -ENOENT;
++	if (!dvfm_find_op(index, &p)) {
++		write_lock(&dvfm_op_list->lock);
++		/* remove device ID */
++		clear_bit(dev_idx, (void *)&p->device);
++		write_unlock(&dvfm_op_list->lock);
++#ifndef CONFIG_BPMD
++		dvfm_driver->enable_op(dvfm_driver->priv, index, RELATION_LOW);
++#endif
++
++	}
++	return 0;
++}
++
++/*
++ * Device set constraint on OP
++ */
++int __dvfm_disable_op(int index, int dev_idx)
++{
++	struct op_info *p = NULL;
++	int num;
++
++	/* check whether dvfm is enabled */
++	if (!dvfm_driver || !dvfm_driver->count)
++		return -EINVAL;
++	/* only registered device can invoke DVFM operation */
++	if ((dev_idx >= DVFM_MAX_DEVICE) || dev_idx < 0)
++		return -ENOENT;
++	num = dvfm_driver->count(dvfm_driver->priv, dvfm_op_list);
++	if (num <= index)
++		return -ENOENT;
++	if (!dvfm_find_op(index, &p)) {
++		write_lock(&dvfm_op_list->lock);
++		/* set device ID */
++		set_bit(dev_idx, (void *)&p->device);
++		write_unlock(&dvfm_op_list->lock);
++		dvfm_driver->disable_op(dvfm_driver->priv, index, RELATION_LOW);
++	}
++	return 0;
++}
++
++int __dvfm_disable_op2(int index, int dev_idx)
++{
++        struct op_info *p = NULL;
++        int num;
++
++        if (!dvfm_driver || !dvfm_driver->count) {
++                return -ENOENT;
++        }
++        num = dvfm_driver->count(dvfm_driver->priv, dvfm_op_list);
++        if (num <= index)
++                return -ENOENT;
++        if (!dvfm_find_op(index, &p)) {
++                write_lock(&dvfm_op_list->lock);
++		set_bit(dev_idx, (void *)&p->device);
++                write_unlock(&dvfm_op_list->lock);
++        }
++        return 0;
++}
++
++int dvfm_enable_op(int index, int dev_idx)
++{
++#ifdef CONFIG_BPMD
++	bpm_enable_op(index, dev_idx);
++#else
++	__dvfm_enable_op(index, dev_idx);
++#endif
++	return 0;
++}
++
++int dvfm_disable_op(int index, int dev_idx)
++{
++#ifdef CONFIG_BPMD
++	bpm_disable_op(index, dev_idx);
++#else
++        __dvfm_disable_op(index, dev_idx);
++#endif
++        return 0;
++}
++
++EXPORT_SYMBOL(dvfm_enable_op);
++EXPORT_SYMBOL(dvfm_disable_op);
++
++int __dvfm_enable_op_name(char *name, int dev_idx)
++{
++	struct op_info *p = NULL;
++	int index;
++
++	if (!dvfm_driver || !dvfm_driver->name || !name)
++		return -EINVAL;
++	/* only registered device can invoke DVFM operation */
++	if ((dev_idx >= DVFM_MAX_DEVICE) || dev_idx < 0)
++		return -ENOENT;
++	list_for_each_entry(p, &dvfm_op_list->list, list) {
++		if (!strcmp(dvfm_driver->name(dvfm_driver->priv, p), name)) {
++			index = p->index;
++			write_lock(&dvfm_op_list->lock);
++			clear_bit(dev_idx, (void *)&p->device);
++			write_unlock(&dvfm_op_list->lock);
++			dvfm_driver->enable_op(dvfm_driver->priv,
++					index, RELATION_LOW);
++			break;
++		}
++	}
++	return 0;
++}
++
++int __dvfm_disable_op_name(char *name, int dev_idx)
++{
++	struct op_info *p = NULL;
++	int index;
++
++	if (!dvfm_driver || !dvfm_driver->name || !name)
++		return -EINVAL;
++	/* only registered device can invoke DVFM operation */
++	if ((dev_idx >= DVFM_MAX_DEVICE) || dev_idx < 0)
++		return -ENOENT;
++	list_for_each_entry(p, &dvfm_op_list->list, list) {
++		if (!strcmp(dvfm_driver->name(dvfm_driver->priv, p), name)) {
++			index = p->index;
++			write_lock(&dvfm_op_list->lock);
++			set_bit(dev_idx, (void *)&p->device);
++			write_unlock(&dvfm_op_list->lock);
++			dvfm_driver->disable_op(dvfm_driver->priv,
++					index, RELATION_LOW);
++			break;
++		}
++	}
++	return 0;
++}
++
++/*
++EXPORT_SYMBOL(dvfm_enable_op_name);
++EXPORT_SYMBOL(dvfm_disable_op_name);
++*/
++
++int _dvfm_enable_op_name(char *name, int dev_idx, char *sid)
++{
++	int ret;
++#ifdef CONFIG_BPMD
++	ret = bpm_enable_op_name(name, dev_idx, sid); 
++#else
++	ret = __dvfm_enable_op_name(name, dev_idx);
++#endif
++	return ret; 
++}
++
++int _dvfm_disable_op_name(char *name, int dev_idx, char *sid)
++{
++	int ret;
++#ifdef CONFIG_BPMD
++        ret = bpm_disable_op_name(name, dev_idx, sid);
++#else
++        ret = __dvfm_disable_op_name(name, dev_idx);
++#endif
++	return ret;
++}
++ 
++EXPORT_SYMBOL(_dvfm_enable_op_name);
++EXPORT_SYMBOL(_dvfm_disable_op_name);
++
++/* Only enable those safe operating point */
++int dvfm_enable(int dev_idx)
++{
++	printk(KERN_WARNING "dvfm_enable() is not preferred\n");
++	WARN_ON(1);
++	if (!dvfm_driver || !dvfm_driver->count || !dvfm_driver->enable_dvfm)
++		return -ENOENT;
++	return dvfm_driver->enable_dvfm(dvfm_driver->priv, dev_idx);
++}
++
++/* return whether the result is zero */
++int dvfm_disable(int dev_idx)
++{
++	printk(KERN_WARNING "dvfm_disable() is not preferred\n");
++	WARN_ON(1);
++	if (!dvfm_driver || !dvfm_driver->count || !dvfm_driver->disable_dvfm)
++		return -ENOENT;
++	return dvfm_driver->disable_dvfm(dvfm_driver->priv, dev_idx);
++}
++
++/* return whether the result is zero */
++int dvfm_enable_pm(void)
++{
++	return atomic_inc_and_test(&lp_count);
++}
++
++/* return whether the result is zero */
++int dvfm_disable_pm(void)
++{
++	return atomic_dec_and_test(&lp_count);
++}
++
++int dvfm_notifier_frequency(struct dvfm_freqs *freqs, unsigned int state)
++{
++	int ret;
++
++	switch (state) {
++	case DVFM_FREQ_PRECHANGE:
++		ret = atomic_notifier_call_chain(&dvfm_freq_notifier_list,
++					DVFM_FREQ_PRECHANGE, freqs);
++		if (ret != NOTIFY_DONE)
++			pr_debug("Failure in device driver before "
++				"switching frequency\n");
++		break;
++	case DVFM_FREQ_POSTCHANGE:
++		ret = atomic_notifier_call_chain(&dvfm_freq_notifier_list,
++					DVFM_FREQ_POSTCHANGE, freqs);
++		if (ret != NOTIFY_DONE)
++			pr_debug("Failure in device driver after "
++				"switching frequency\n");
++		break;
++	default:
++		ret = -EINVAL;
++	}
++	return ret;
++}
++
++int dvfm_register_notifier(struct notifier_block *nb, unsigned int list)
++{
++	int ret;
++
++	switch (list) {
++	case DVFM_FREQUENCY_NOTIFIER:
++		ret = atomic_notifier_chain_register(
++				&dvfm_freq_notifier_list, nb);
++		break;
++	default:
++		ret = -EINVAL;
++	}
++	return ret;
++}
++EXPORT_SYMBOL(dvfm_register_notifier);
++
++int dvfm_unregister_notifier(struct notifier_block *nb, unsigned int list)
++{
++	int ret;
++
++	switch (list) {
++	case DVFM_FREQUENCY_NOTIFIER:
++		ret = atomic_notifier_chain_unregister(
++				&dvfm_freq_notifier_list, nb);
++		break;
++	default:
++		ret = -EINVAL;
++	}
++	return ret;
++}
++EXPORT_SYMBOL(dvfm_unregister_notifier);
++
++/*
++ * add device into trace list
++ * return device index
++ */
++static int add_device(char *name)
++{
++	struct dvfm_trace_info	*entry = NULL, *new = NULL;
++	int min;
++
++	min = find_first_zero_bit(&dvfm_trace_list.device, DVFM_MAX_DEVICE);
++	if (min == DVFM_MAX_DEVICE)
++		return -EINVAL;
++
++	/* If device trace table is NULL */
++	new = kzalloc(sizeof(struct dvfm_trace_info), GFP_ATOMIC);
++	if (new == NULL)
++		goto out_mem;
++	/* add new item */
++	strcpy(new->name, name);
++	new->index = min;
++	/* insert the new item in increasing order */
++	list_for_each_entry(entry, &dvfm_trace_list.list, list) {
++		if (entry->index > min) {
++			list_add_tail(&(new->list), &(entry->list));
++			goto inserted;
++		}
++	}
++	list_add_tail(&(new->list), &(dvfm_trace_list.list));
++inserted:
++	set_bit(min, (void *)&dvfm_trace_list.device);
++
++	return min;
++out_mem:
++	return -ENOMEM;
++}
++
++/*
++ * Query the device number that registered in DVFM
++ */
++int dvfm_query_device_num(void)
++{
++	int count = 0;
++	struct dvfm_trace_info *entry = NULL;
++
++	read_lock(&dvfm_trace_list.lock);
++	list_for_each_entry(entry, &dvfm_trace_list.list, list) {
++		count++;
++	}
++	read_unlock(&dvfm_trace_list.lock);
++	return count;
++}
++EXPORT_SYMBOL(dvfm_query_device_num);
++
++/*
++ * Query all device name that registered in DVFM
++ */
++int dvfm_query_device_list(void *mem, int len)
++{
++	int count = 0, size;
++	struct dvfm_trace_info *entry = NULL;
++	struct name_list *p = (struct name_list *)mem;
++
++	count = dvfm_query_device_num();
++	size = sizeof(struct name_list);
++	if (len < count * size)
++		return -ENOMEM;
++
++	read_lock(&dvfm_trace_list.lock);
++	list_for_each_entry(entry, &dvfm_trace_list.list, list) {
++		p->id = entry->index;
++		strcpy(p->name, entry->name);
++		p++;
++	}
++	read_unlock(&dvfm_trace_list.lock);
++	return 0;
++}
++EXPORT_SYMBOL(dvfm_query_device_list);
++
++/*
++ * Device driver register itself to DVFM before any operation.
++ * The number of registered device is limited in 32.
++ */
++int dvfm_register(char *name, int *id)
++{
++	struct dvfm_trace_info	*p = NULL;
++	int len, idx;
++
++	if (name == NULL)
++		return -EINVAL;
++
++	/* device name is stricted in 32 bytes */
++	len = strlen(name);
++	if (len > DVFM_MAX_NAME)
++		len = DVFM_MAX_NAME;
++	write_lock(&dvfm_trace_list.lock);
++	list_for_each_entry(p, &dvfm_trace_list.list, list) {
++		if (!strcmp(name, p->name)) {
++			/*
++			 * Find device in device trace table
++			 * Skip to allocate new ID
++			 */
++			*id = p->index;
++			goto out;
++		}
++	}
++	idx = add_device(name);
++	if (idx < 0)
++		goto out_num;
++	*id = idx;
++out:
++	write_unlock(&dvfm_trace_list.lock);
++	return 0;
++out_num:
++	write_unlock(&dvfm_trace_list.lock);
++	return -EINVAL;
++}
++EXPORT_SYMBOL(dvfm_register);
++
++/*
++ * Release the device and free the device index.
++ */
++int dvfm_unregister(char *name, int *id)
++{
++	struct op_info *q = NULL;
++	struct dvfm_trace_info	*p = NULL;
++	int len, num, i;
++
++	if (!dvfm_driver || !dvfm_driver->count || (name == NULL))
++		return -EINVAL;
++
++	/* device name is stricted in 32 bytes */
++	len = strlen(name);
++	if (len > DVFM_MAX_NAME)
++		len = DVFM_MAX_NAME;
++
++	num = dvfm_driver->count(dvfm_driver->priv, dvfm_op_list);
++
++	write_lock(&dvfm_trace_list.lock);
++	if (list_empty(&dvfm_trace_list.list))
++		goto out;
++	list_for_each_entry(p, &dvfm_trace_list.list, list) {
++		if (!strncmp(name, p->name, len)) {
++			for (i = 0; i < num; ++i) {
++			        if (!dvfm_find_op(i, &q)) {
++                			write_lock(&dvfm_op_list->lock);
++                			if (test_bit(p->index, (void *)&q->device)) {
++						printk(KERN_ERR "%s uses PM interface unrightly, please clean the constraint before quit!\n", name);
++						dvfm_enable_op(i, p->index);
++					}
++			                write_unlock(&dvfm_op_list->lock);
++				}
++			}
++
++			/* clear the device index */
++			clear_bit(*id, (void *)&dvfm_trace_list.device);
++			*id = -1;
++			list_del(&p->list);
++			kfree(p);
++			break;
++		}
++	}
++	write_unlock(&dvfm_trace_list.lock);
++	return 0;
++out:
++	write_unlock(&dvfm_trace_list.lock);
++	return -ENOENT;
++}
++EXPORT_SYMBOL(dvfm_unregister);
++
++#ifndef CONFIG_BPMD
++static int dvfm_add(struct sys_device *sys_dev)
++{
++	int i, n;
++	int ret;
++
++	n = ARRAY_SIZE(dvfm_attr);
++	for (i = 0; i < n; i++) {
++		ret = sysfs_create_file(&(sys_dev->kobj), dvfm_attr[i]);
++		if (ret)
++			return -EIO;
++	}
++	return 0;
++}
++
++static int dvfm_rm(struct sys_device *sys_dev)
++{
++	int i, n;
++	n = ARRAY_SIZE(dvfm_attr);
++	for (i = 0; i < n; i++) {
++		sysfs_remove_file(&(sys_dev->kobj), dvfm_attr[i]);
++	}
++	return 0;
++}
++
++static int dvfm_suspend(struct sys_device *sysdev, pm_message_t pmsg)
++{
++	return 0;
++}
++
++static int dvfm_resume(struct sys_device *sysdev)
++{
++	return 0;
++}
++
++static struct sysdev_driver dvfm_sysdev_driver = {
++	.add		= dvfm_add,
++	.remove		= dvfm_rm,
++	.suspend	= dvfm_suspend,
++	.resume		= dvfm_resume,
++};
++#endif
++
++int dvfm_register_driver(struct dvfm_driver *driver_data, struct info_head *op_list)
++{
++	int ret = 0;
++	if (!driver_data || !driver_data->set)
++		return -EINVAL;
++	if (dvfm_driver)
++		return -EBUSY;
++	dvfm_driver = driver_data;
++
++	if (!op_list)
++		return -EINVAL;
++	dvfm_op_list = op_list;
++
++#ifndef CONFIG_BPMD
++	/* enable_op need to invoke dvfm operation */
++	dvfm_register("User", &dvfm_dev_idx);
++	ret = sysdev_driver_register(&cpu_sysdev_class, &dvfm_sysdev_driver);
++#endif
++	return ret;
++}
++
++int dvfm_unregister_driver(struct dvfm_driver *driver)
++{
++#ifndef CONFIG_BPMD
++	sysdev_driver_unregister(&cpu_sysdev_class, &dvfm_sysdev_driver);
++	dvfm_unregister("User", &dvfm_dev_idx);
++#endif
++	dvfm_driver = NULL;
++	return 0;
++}
++
++unsigned int NextWakeupTimeAbs;
++unsigned int AppsSyncEnabled = 0;
++
++//this function should be called form ACIPC driver when comm relenquish events occurs
++int dvfm_notify_next_comm_wakeup_time(unsigned int NextWakeupTimeRel)
++{
++	unsigned int TimeStamp;
++
++	TimeStamp = dvfm_driver->read_time();
++
++	if (NextWakeupTimeRel == 0)
++	{
++		AppsSyncEnabled = 0;
++	}
++	else
++	{
++		AppsSyncEnabled = 1;
++	}
++	//we receive the next relative comm wakeup time and add to current TS to get the absolute time of the next comm wakeup.
++	//this value is stored in a global variable for future use. this should be done every time the comm side goes to D2
++	NextWakeupTimeAbs = NextWakeupTimeRel + TimeStamp;
++	return 0;
++}
++
++//this function should be called from mspm_idle when we want to go to D2 to check when the next wakeup will occur.
++int dvfm_is_comm_wakep_near(void)
++{
++	unsigned int TimeStamp;
++	TimeStamp = dvfm_driver->read_time();
++
++	//if the feature is not enabled we should not prevent D2.
++	if (!AppsSyncEnabled)
++		return 0;
++
++	if (NextWakeupTimeAbs - TimeStamp < APPS_COMM_D2_THRESHOLD)
++	{
++		return (NextWakeupTimeAbs - TimeStamp);     //preventing D2
++	}
++	else
++	{
++		return 0;    //allowing D2
++	}
++}
++
++MODULE_DESCRIPTION("Basic DVFM support for Monahans");
++MODULE_LICENSE("GPL");
+diff -ur linux-2.6.32/arch/arm/mach-pxa/include/mach/bpm.h kernel/arch/arm/mach-pxa/include/mach/bpm.h
+--- linux-2.6.32/arch/arm/mach-pxa/include/mach/bpm.h	2009-12-13 12:59:07.871960663 +0200
++++ kernel/arch/arm/mach-pxa/include/mach/bpm.h	2009-12-12 16:09:26.446281263 +0200
+@@ -0,0 +1,57 @@
++/*
++ * Copyright (C) 2003-2004 Intel Corporation.
++ *
++ * This software program is licensed subject to the GNU General Public License
++ * (GPL).Version 2,June 1991, available at http://www.fsf.org/copyleft/gpl.html
++ *
++ *
++ * (C) Copyright 2006 Marvell International Ltd.
++ * All Rights Reserved
++ *
++ * (C) Copyright 2008 Borqs Corporation.
++ * All Rights Reserved
++ */
++
++#ifndef __BPM_H__
++#define __BPM_H__
++
++#ifdef __KERNEL__
++
++/* 10 BPM event max */
++#define MAX_BPM_EVENT_NUM		10	
++#define INFO_SIZE			128
++
++struct bpm_event {
++	int type;			/* What type of IPM events. */
++	int kind;			/* What kind, or sub-type of events. */
++	unsigned char info[INFO_SIZE];	/* events specific data. */
++};
++
++/* IPM events queue */
++struct bpm_event_queue{
++        int head;
++        int tail;
++        int len;
++        struct bpm_event bpmes[MAX_BPM_EVENT_NUM];
++        wait_queue_head_t waitq;
++};
++
++/* IPM event types. */
++#define IPM_EVENT_PROFILER		0x7	/* Profiler events. */
++
++#define IPM_EVENT_BLINK			(0xA0)
++
++/* IPM event kinds. */
++#define IPM_EVENT_IDLE_PROFILER		0x1
++#define IPM_EVENT_PERF_PROFILER		0x2
++
++#define IPM_EVENT_BLINK_SPEEDUP		(0x1)
++
++/* IPM event infos, not defined yet. */
++#define IPM_EVENT_NULLINFO		0x0
++
++/* IPM functions */
++extern int bpm_event_notify(int type, int kind, void *info, unsigned int info_len);
++#endif
++
++#endif
+diff -ur linux-2.6.32/arch/arm/mach-pxa/include/mach/dvfm.h kernel/arch/arm/mach-pxa/include/mach/dvfm.h
+--- linux-2.6.32/arch/arm/mach-pxa/include/mach/dvfm.h	2009-12-13 12:59:13.655291426 +0200
++++ kernel/arch/arm/mach-pxa/include/mach/dvfm.h	2009-12-12 16:09:26.446281263 +0200
+@@ -0,0 +1,226 @@
++/*
++ * Copyright (C) 2003-2004 Intel Corporation.
++ *
++ * This software program is licensed subject to the GNU General Public License
++ * (GPL).Version 2,June 1991, available at http://www.fsf.org/copyleft/gpl.html
++ *
++
++ *(C) Copyright 2006 Marvell International Ltd.
++ * All Rights Reserved
++ */
++
++#ifndef DVFM_H
++#define DVFM_H
++
++
++#ifdef __KERNEL__
++enum {
++	FV_NOTIFIER_QUERY_SET = 1,
++	FV_NOTIFIER_PRE_SET = 2,
++	FV_NOTIFIER_POST_SET = 3,
++};
++
++
++#define MAXTOKENS			80
++#define CONSTRAINT_NAME_LEN		20
++
++#define DVFM_MAX_NAME			32
++#define DVFM_MAX_DEVICE			32
++
++#define DVFM_FREQUENCY_NOTIFIER		0
++#define	DVFM_LOWPOWER_NOTIFIER		1
++
++#define DVFM_FREQ_PRECHANGE		0
++#define DVFM_FREQ_POSTCHANGE		1
++
++#define DVFM_LOWPOWER_PRECHANGE		0
++#define DVFM_LOWPOWER_POSTCHANGE	1
++#define APPS_COMM_D2_THRESHOLD 326
++
++/* set the lowest operating point that is equal or higher than specified */
++#define RELATION_LOW			0
++/* set the highest operating point that is equal or lower than specified */
++#define RELATION_HIGH			1
++/* set the specified operating point */
++#define RELATION_STICK			2
++
++/* Both of these states are used in statistical calculation */
++#define CPU_STATE_RUN			1
++#define CPU_STATE_IDLE			2
++
++/*
++ * operating point definition
++ */
++
++struct op_info {
++	void 			*op;
++	struct list_head	list;
++	unsigned int		index;
++	unsigned int		device;	/* store the device ID blocking OP */
++};
++
++struct dvfm_freqs {
++	unsigned int	old;	/* operating point index */
++	unsigned int	new;	/* operating point index */
++	struct op_info	old_info;
++	struct op_info	new_info;
++	unsigned int	flags;
++};
++
++struct op_freq {
++	unsigned int 	cpu_freq;
++};
++
++struct dvfm_op {
++	int		index;
++	int		count;
++	unsigned int	cpu_freq;
++	const char*	name;		
++};
++
++struct info_head {
++	struct list_head	list;
++	rwlock_t		lock;
++	unsigned int		device;	/* store the registerred device ID */
++};
++
++struct head_notifier {
++	spinlock_t		lock;
++	struct notifier_block	*head;
++};
++
++/**
++ * struct dvfm_lock - the lock struct of dvfm 
++ * @lock: the spin lock struct.
++ * @flags: the flags for spin lock.
++ * @count: the count of dvfm_disable_op_name() or dvfm_enable_op_name()
++ *
++ * This struct is used for the mutex lock of dvfm_disable_op_name() and 
++ * dvfm_enable_op_name(). The caller can not call dvfm_enable_op_name() 
++ * without call dvfm_disable_op_name() before, so the caller of 
++ * dvfm_disable_op_name() and dvfm_enable_op_name() must record the 
++ * called times of these two functions. 
++ */
++struct dvfm_lock {
++	spinlock_t	lock;
++	unsigned long	flags;
++	int		dev_idx;
++	int		count;
++};
++
++/*
++ * Store the dev_id and dev_name.
++ * Registered device number can't be larger than 32.
++ */
++struct dvfm_trace_info {
++	struct list_head	list;
++	int			index;		/* index is [0,31] */
++	unsigned int		dev_id;		/* dev_id == 1 << index */
++	char			name[DVFM_MAX_NAME];
++};
++
++
++struct dvfm_driver {
++	int	(*get_opinfo)(void *driver_data, void *info);
++	int	(*count)(void *driver_data, struct info_head *op_table);
++	int	(*set)(void *driver_data, struct dvfm_freqs *freq, unsigned int new,
++			unsigned int relation);
++	int	(*dump)(void *driver_data, struct op_info *md, char *buf);
++	char *	(*name)(void *driver_data, struct op_info *md);
++	int	(*request_set)(void *driver_data, int index);
++	int	(*enable_dvfm)(void *driver_data, int dev_id);
++	int	(*disable_dvfm)(void *driver_data, int dev_id);
++	int	(*enable_op)(void *driver_data, int index, int relation);
++	int	(*disable_op)(void *driver_data, int index, int relation);
++	int	(*volt_show)(void *driver_data, char *buf);
++#ifdef CONFIG_CPU_PXA310
++	int	(*freq_show)(void *driver_date, struct op_info *md, char *buf);
++#endif
++	unsigned int	(*ticks_to_usec)(unsigned int);
++	unsigned int	(*ticks_to_sec)(unsigned int);
++	unsigned int	(*read_time)(void);
++	int	(*get_freq)(void* driver_data, struct op_info *md, struct op_freq *freq);
++	int 	(*check_active_op)(void *driver_data, struct op_info *md);
++	void	*priv;
++};
++
++extern struct dvfm_driver *dvfm_driver;
++extern struct info_head *dvfm_op_list;
++extern unsigned int op_nums;
++
++extern int dvfm_notifier_frequency(struct dvfm_freqs *freqs, unsigned int state);
++extern int dvfm_notifier_lowpower(struct dvfm_freqs *freqs, unsigned int state);
++extern int dvfm_register_notifier(struct notifier_block *nb, unsigned int list);
++extern int dvfm_unregister_notifier(struct notifier_block *nb, unsigned int list);
++extern int dvfm_register_driver(struct dvfm_driver *driver_data, struct info_head *op_list);
++extern int dvfm_unregister_driver(struct dvfm_driver *driver);
++extern int dvfm_register(char *name, int *);
++extern int dvfm_unregister(char *name, int *);
++extern int dvfm_query_device_num(void);
++extern int dvfm_query_device_list(void *, int);
++
++extern int dvfm_enable_op(int, int);
++extern int dvfm_disable_op(int, int);
++extern int dvfm_enable(int);
++extern int dvfm_enable_op_name(char *, int);
++extern int dvfm_disable_op_name(char *, int);
++extern int dvfm_disable(int);
++extern int dvfm_dump_op(int, char*);
++
++extern int dvfm_set_op(struct dvfm_freqs *, unsigned int, unsigned int);
++extern int dvfm_get_op(struct op_info **);
++extern int dvfm_get_op_freq(int, struct op_freq *);
++extern int dvfm_check_active_op(int);
++extern int dvfm_get_defop(void);
++extern int dvfm_get_opinfo(int, struct op_info **);
++extern int dvfm_request_op(int);
++extern int dvfm_op_count(void);
++extern int dvfm_find_op(int, struct op_info **);
++extern int dvfm_trace(char *);
++extern int dvfm_add_event(int, int, int, int);
++extern int dvfm_add_timeslot(int, int);
++extern int calc_switchtime_start(int, int, unsigned int);
++extern int calc_switchtime_end(int, int, unsigned int);
++
++//hanling comm apps sync
++extern int dvfm_notify_next_comm_wakeup_time(unsigned int NextWakeupTimeRel);
++extern int dvfm_is_comm_wakep_near(void);
++
++extern const char* dvfm_get_op_name(int);
++
++/**
++ * dvfm_disable_op_name: - disable the operating point by its name.
++ * @name:   the operating point's name.
++ *
++ * Context: process and interrupt. 
++ *
++ * disable the operating points by op's name, op name set includes
++ * "D0CS","156M","208M","416M","624M" and "D2".
++ *
++ * Returns zero on success, else negative errno.
++ */
++#define dvfm_disable_op_name(name, dev_idx) \
++	(_dvfm_disable_op_name(name, dev_idx, __FILE__))
++
++extern int _dvfm_disable_op_name(char *name, int dev_idx, char *sid);
++
++/**
++ * dvfm_enable_op_name: - enable the operating point by its name.
++ * @name:   the operating point's name.
++ *
++ * Context: process and interrupt. 
++ *
++ * enable the operating points by op's name, op name set includes
++ * "D0CS","156M","208M","416M","624M" and "D2".
++ *
++ * Returns zero on success, else negative errno.
++ */
++#define dvfm_enable_op_name(name, dev_idx) \
++	(_dvfm_enable_op_name(name, dev_idx, __FILE__))
++
++extern int _dvfm_enable_op_name(char *name, int dev_idx, char *sid);
++
++#endif
++
++#endif
++
+diff -ur linux-2.6.32/arch/arm/mach-pxa/include/mach/mspm_prof.h kernel/arch/arm/mach-pxa/include/mach/mspm_prof.h
+--- linux-2.6.32/arch/arm/mach-pxa/include/mach/mspm_prof.h	2009-12-13 12:59:18.941953014 +0200
++++ kernel/arch/arm/mach-pxa/include/mach/mspm_prof.h	2009-12-12 16:09:26.456281390 +0200
+@@ -0,0 +1,66 @@
++/*
++ * PXA Performance profiler and Idle profiler Routines
++ *
++ * Copyright (c) 2003 Intel Corporation.
++ *
++ * This program is free software; you can redistribute it and/or modify
++ * it under the terms of the GNU General Public License version 2 as
++ * published by the Free Software Foundation.
++ *
++ * (C) Copyright 2006 Marvell International Ltd.
++ * All Rights Reserved
++ */
++
++#ifndef MSPM_PROF_H
++#define MSPM_PROF_H
++
++#include <mach/pmu.h>
++#include <mach/xscale-pmu.h>
++
++#define IPM_IDLE_PROFILER	1
++#define IPM_PMU_PROFILER	2
++
++struct ipm_profiler_result {
++	struct pmu_results pmu;
++	unsigned int busy_ratio;	/* CPU busy ratio */
++	unsigned int mips;
++	unsigned int window_size;
++};
++
++struct ipm_profiler_arg {
++	unsigned int size;		/* size of ipm_profiler_arg */
++	unsigned int flags;
++	unsigned int window_size;	/* in microseconds */
++	unsigned int pmn0;
++	unsigned int pmn1;
++	unsigned int pmn2;
++	unsigned int pmn3;
++};
++
++#ifdef __KERNEL__
++extern volatile int	hlt_counter;
++
++#define OSCR_MASK		~(1UL)
++
++#undef MAX_OP_NUM
++#define MAX_OP_NUM		20
++
++/* The minimum sample window is 20ms, the default window is 100ms */
++#define MIN_SAMPLE_WINDOW	20
++#define DEF_SAMPLE_WINDOW	100
++
++#define DEF_HIGH_THRESHOLD	80
++#define DEF_LOW_THRESHOLD	20
++
++extern int mspm_add_event(int op, int cpu_idle);
++extern int mspm_prof_init(void);
++extern void mspm_prof_exit(void);
++
++extern int bpm_event_notify(int type, int kind, void *info,
++		unsigned int info_len);
++//extern int (*pipm_start_pmu)(struct ipm_profiler_arg *arg);
++//extern int (*pipm_stop_pmu)(void);
++#endif
++
++#endif
++
+diff -ur linux-2.6.32/arch/arm/mach-pxa/include/mach/pmu.h kernel/arch/arm/mach-pxa/include/mach/pmu.h
+--- linux-2.6.32/arch/arm/mach-pxa/include/mach/pmu.h	2009-12-13 12:59:24.521951391 +0200
++++ kernel/arch/arm/mach-pxa/include/mach/pmu.h	2009-12-12 16:09:26.459612243 +0200
+@@ -0,0 +1,555 @@
++/*
++ * "This software program is available to you under a choice of one of two
++ * licenses.  You may choose to be licensed under either the GNU General Public
++ * License (GPL) Version 2, June 1991, available at
++ * http://www.fsf.org/copyleft/gpl.html, or the BSD License, the text of
++ * which follows:
++ *
++ * Copyright (c) 1996-2005, Intel Corporation. All rights reserved.
++ *
++ * Redistribution and use in source and binary forms, with or without
++ * modification, are permitted provided that the following conditions are met:
++ *
++ * Redistributions of source code must retain the above copyright notice, this
++ * list of conditions and the following disclaimer.
++ *
++ * Redistributions in binary form must reproduce the above copyright notice, this
++ * list of conditions and the following disclaimer in the documentation and/or
++ * other materials provided with the distribution.
++ *
++ * Neither the name of the Intel Corporation ("Intel") nor the names of its
++ * contributors may be used to endorse or promote products derived from this
++ * software without specific prior written permission.
++ *
++ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
++ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
++ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
++ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
++ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
++ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
++ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
++ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
++ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
++ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE."
++ */
++
++/*
++ *  FILENAME: pmu.h
++ *
++ *  CORE STEPPING:
++ *
++ *  PURPOSE: contains all PMU specific macros, typedefs, and prototypes.
++ *           Declares no storage.
++ */
++
++#ifndef __PMU_H__
++#define __PMU_H__
++
++/* PMU Performance Monitor Control Register (PMNC) */
++#define PMU_ID				(0x24u << 24)
++#define PMU_COUNTERS_DISALBLE		(1u<<4)
++#define PMU_CLOCK_DIVIDER		(1u<<3)
++#define PMU_CLOCK_RESET			(1u<<2)
++#define PMU_COUNTERS_RESET		(1u<<1)
++#define PMU_3_COUNTERS_ENABLE		(1u<<0)
++#define PMU_COUNTERS_ENABLE		(1u<<0)
++
++/* INTEN & FLAG Registers bit definition*/
++#define PMU_CLOCK_COUNT			(1u<<0)
++#define PMU_COUNT_0			(1u<<1)
++#define PMU_COUNT_1			(1u<<2)
++#define PMU_COUNT_2			(1u<<3)
++#define PMU_COUNT_3			(1u<<4)
++
++/*Events combination*/
++/*!evtCount0/2:0x7(instruction count), evtCount1/3:0x0(ICache miss)*/
++#define PMU_EVTCOUNT_1			(0x0007)
++/*!evtCount0/2:0xA(DCache Access), evtCount1/3:0xB(DCache miss)*/
++#define PMU_EVTCOUNT_2			(0x0B0A)
++/*!evtCount0/2:0x1(ICache cannot deliver), evtCount1/3:0x0(ICache miss)*/
++#define PMU_EVTCOUNT_3			(0x0001)
++/*!evtCount0/2:0xB(DBufer stall duration), evtCount1/3:0x9(Dbuffer stall)*/
++#define PMU_EVTCOUNT_4			(0x090B)
++/*!evtCount0/2:0x2(data stall), evtCount1/3:0xC(DCache writeback)*/
++#define PMU_EVTCOUNT_5			(0x0C02)
++/*!evtCount0/2:0x7(instruction count), evtCount1/3:0x3(ITLB miss)*/
++#define PMU_EVTCOUNT_6			(0x0307)
++/*!evtCount0/2:0xA(DCache Access), evtCount/31:0x4(DTLB miss)*/
++#define PMU_EVTCOUNT_7			(0x040A)
++
++/* PXA3xx/PXA900 PML event selector register offset */
++#define PML_ESEL_0_OFF			(0x0)
++#define PML_ESEL_1_OFF			(0x4)
++#define PML_ESEL_2_OFF			(0x8)
++#define PML_ESEL_3_OFF			(0xC)
++#define PML_ESEL_4_OFF			(0x10)
++#define PML_ESEL_5_OFF			(0x14)
++#define PML_ESEL_6_OFF			(0x18)
++#define PML_ESEL_7_OFF			(0x1C)
++
++enum {
++	PMU_PMNC = 0,
++	PMU_CCNT,
++	PMU_PMN0,
++	PMU_PMN1,
++	PMU_PMN2,
++	PMU_PMN3,
++	PMU_INTEN,
++	PMU_FLAG,
++	PMU_EVTSEL
++};
++
++/*
++ * PMU and PML Event
++ */
++enum {
++	PMU_EVENT_INVALIDATE=0xFFFFFFFFu,
++
++	/*!< L1 Instruction cache miss requires fetch from external memory */
++	PMU_EVENT_L1_INSTRUCTION_MISS=0x0u,
++
++	/*!< L1 Instruction cache cannot deliver an instruction. this indicate
++	 * an instruction cache or TLB miss. This event will occur eveyr cycle
++	 * in which the condition is present
++	 */
++	PMU_EVENT_L1_INSTRUCTION_NOT_DELIVER,
++
++	/*!< Stall due to a data dependency. This event will occur every cycle
++	 * in which the condition is present
++	 */
++	PMU_EVENT_STALL_DATA_DEPENDENCY,
++
++	/*!< Instruction TLB miss*/
++	PMU_EVENT_INSTRUCTION_TLB_MISS,
++
++	/*!< Data TLB miss*/
++	PMU_EVENT_DATA_TLB_MISS,
++
++	/*!< Branch instruction retired, branch may or many not have changed
++	 * program flow. (Counts only B and BL instruction, in both ARM and
++	 * Thumb mode)
++	 */
++	PMU_EVENT_BRANCH_RETIRED,
++
++	/*!< Branch mispredicted. Counts only B and BL instructions, in both
++	 * ARM and Thumb mode
++	 */
++	PMU_EVENT_BRANCH_MISPREDICTED,
++
++	/*!< Instruction retired. This event will occur every cycle in which
++	 * the condition is present
++	 */
++	PMU_EVENT_INSTRUCTION_RETIRED,
++
++	/*!< L1 Data cache buffer full stall. This event will occur every
++	 * cycle in which the condition is present.
++	 */
++	PMU_EVENT_L1_DATA_STALL,
++
++	/*!< L1 Data cache buffer full stall. This event occur for each
++	 * contiguous sequence of this type of stall
++	 */
++	PMU_EVENT_L1_DATA_STALL_C,
++
++	/*!< L1 Data cache access, not including Cache Operations. All data
++	 * accesses are treated as cacheable accessses and are counted here
++	 * even if the cache is not enabled
++	 */
++	PMU_EVENT_L1_DATA_ACCESS,
++
++	/*!< L1 Data cache miss, not including Cache Operations. All data
++	 * accesses are treated as cachedable accesses and are counted as
++	 * misses if the data cache is not enable
++	 */
++	PMU_EVENT_L1_DATA_MISS,
++
++	/*!< L1 data cache write-back. This event occures once for each line
++	 * that is written back from the cache
++	 */
++	PMU_EVENT_L1_DATA_WRITE_BACK,
++
++	/*!< Software changed the PC(b bx bl blx and eor sub rsb add adc sbc
++	 * rsc orr mov bic mvn ldm pop) will be counted. The count does not
++	 * increment when an exception occurs and the PC changed to the
++	 * exception address(e.g.. IRQ, FIR, SWI,...)
++	 */
++	PMU_EVENT_SOFTWARE_CHANGED_PC,
++
++	/*!< Branch instruction retired, branch may or may noot have chanaged
++	 * program flow.
++	 * (Count ALL branch instructions, indirect as well as direct)
++	 */
++	PMU_EVENT_BRANCH_RETIRED_ALL,
++
++	/*!< Instruction issue cycle of retired instruction. This event is a
++	 * count of the number of core cycle each instruction requires to issue
++	 */
++	PMU_EVENT_INSTRUCTION_CYCLE_RETIRED,
++
++	/*!< All change to the PC. (includes software changes and exceptions*/
++	PMU_EVENT_ALL_CHANGED_PC=0x18,
++
++	/*!< Pipe line flush due to branch mispredict or exception*/
++	PMU_EVENT_PIPE_FLUSH_BRANCH,
++
++	/*!< The core could not issue an instruction due to a backed stall.
++	 * This event will occur every cycle in which the condition is present
++	 */
++	PMU_EVENT_BACKEND_STALL,
++
++	/*!< Multiplier in use. This event will occur every cycle in which
++	 * the multiplier is active
++	 */
++	PMU_EVENT_MULTIPLIER,
++
++	/*!< Multiplier stalled the instruction pipelien due to resource stall.
++	 * This event will occur every cycle in which the condition is present
++	 */
++	PMU_EVENT_MULTIPLIER_STALL_PIPE,
++
++	/*!< Coprocessor stalled the instruction pipeline. This event will
++	 * occur every cycle in which the condition is present
++	 */
++	PMU_EVENT_COPROCESSOR_STALL_PIPE,
++
++	/*!< Data cache stalled the instruction pipeline. This event will
++	 * occur every cycle in which the  condition is present
++	 */
++	PMU_EVENT_DATA_CACHE_STALL_PIPE,
++
++	/*!< Unified L2 Cache request, not including cache operations. This
++	 * event includes table walks, data and instruction reqeusts
++	 */
++	PMU_EVENT_L2_REQUEST=0x20,
++
++	/*!< Unified L2 cache miss, not including cache operations*/
++	PMU_EVENT_L2_MISS=0x23,
++
++	/*!< Address bus transcation*/
++	PMU_EVENT_ADDRESS_BUS=0x40,
++
++	/*!< Self initiated(Core Generated) address bus transaction*/
++	PMU_EVENT_SELF_INITIATED_ADDRESS,
++
++	/*!< Bus clock. This event occurs onece for each bus cycle*/
++	PMU_EVENT_BUS_CLOCK=0x43,
++
++	/*!< Data bus transaction. This event occurs once for
++	 * each data bus cycle
++	 */
++	PMU_EVENT_SELF_INITIATED_DATA=0x47,
++
++	/*!< Data bus transaction. This event occures once for
++	 * each data bus cycle
++	 */
++	PMU_EVENT_BUS_TRANSACTION,
++
++	PMU_EVENT_ASSP_0=0x80,
++	PMU_EVENT_ASSP_1,
++	PMU_EVENT_ASSP_2,
++	PMU_EVENT_ASSP_3,
++	PMU_EVENT_ASSP_4,
++	PMU_EVENT_ASSP_5,
++	PMU_EVENT_ASSP_6,
++	PMU_EVENT_ASSP_7,
++
++	/*!< Power Saving event. This event deactivates the corresponding
++	 * PMU event counter
++	 */
++	PMU_EVENT_POWER_SAVING=0xFF,
++
++	PXA3xx_EVENT_MASK=0x80000000,
++
++	/*!< Core is performing a new instruction fetch.
++	 * e.g. an L2 cache miss.
++	 */
++	PXA3xx_EVENT_CORE_INSTRUCTION_FETCH=PXA3xx_EVENT_MASK,
++
++	/*!< Core is performing a new data fetch*/
++	PXA3xx_EVENT_CORE_DATA_FETCH,
++
++	/*!< Core read request count*/
++	PXA3xx_EVENT_CORE_READ,
++
++	/*!< LCD read request cout*/
++	PXA3xx_EVENT_LCD_READ,
++
++	/*!< DMA read request count*/
++	PXA3xx_EVENT_DMA_READ,
++
++	/*!< Camera interface read request cout*/
++	PXA3xx_EVENT_CAMERA_READ,
++
++	/*!< USB 2.0 read request count*/
++	PXA3xx_EVENT_USB20_READ,
++
++	/*!< 2D grahpic read request count*/
++	PXA3xx_EVENT_2D_READ,
++
++	/*!< USB1.1 host read reqeust count*/
++	PXA3xx_EVENT_USB11_READ,
++
++	/*!< PX1 bus unitization. the number of cycles durring which
++	 * the PX1 bus is occupied
++	 */
++	PXA3xx_EVENT_PX1_UNITIZATION,
++
++	/*!< PX2(sidecar) bus unitization. the number of cycles
++	 * durring which the PX2 bus is occupied
++	 */
++	PXA3xx_EVENT_PX2_UNITIZATION,
++
++	/*!< Dynamic memory queue for Mandris occupied. the number of
++	 * cycles when the DMC queue is not empty
++	 */
++	PXA3xx_EVENT_DMC_NOT_EMPTY=PXA3xx_EVENT_MASK|14,
++
++	/*!< Dynamic memory queue for Mandris occupied by more than 1 request.
++	 * the number of cycles when the DMC queue has 2 or more requests
++	 */
++	PXA3xx_EVENT_DMC_2,
++
++	/*!< Dynamic memory queue for Mandris occupied by more than 2 request.
++	 * the number of cycles when the DMC queue has 3 or more requests
++	 */
++	PXA3xx_EVENT_DMC_3,
++
++	/*!< Dynamic memory queue for Mandris occupied by more than 3 request.
++	 * the number of cycles when the DMC queue is full
++	 */
++	PXA3xx_EVENT_DMC_FULL,
++
++	/*!< Static memory queue for Mandris occupied. the number of cycles
++	 * when the SMC queue is not empty
++	 */
++	PXA3xx_EVENT_SMC_NOT_EMPTY,
++
++	/*!< Static memory queue for Mandris occupied by more than 1 request.
++	 * the number of cycles when the SMC queue has 2 or more requests
++	 */
++	PXA3xx_EVENT_SMC_2,
++
++	/*!< Static memory queue for Mandris occupied by more than 2 request.
++	 * the number of cycles when the SMC queue has 3 or more requests
++	 */
++	PXA3xx_EVENT_SMC_3,
++
++	/*!< Static memory queue for Mandris occupied by more than 3 request.
++	 * the number of cycles when the SMC queue is full
++	 */
++	PXA3xx_EVENT_SMC_FULL,
++
++	/*!< Internal SRAM queue for Mandris occupied. the number of cycles
++	 * when the ISRAM queue is not empty
++	 */
++	PXA3xx_EVENT_ISRAM_NOT_EMPTY=PXA3xx_EVENT_MASK|26,
++
++	/*!< Internal SRAM queue for Mandris occupied by more than 1 request.
++	 * the number of cycles when the ISRAM queue has 2 or more requests
++	 */
++	PXA3xx_EVENT_ISRAM_2,
++
++	/*!< Internal SRAM queue for Mandris occupied by more than 2 request.
++	 * the number of cycles when the ISRAM queue has 3 or more requests
++	 */
++	PXA3xx_EVENT_ISRAM_3,
++
++	/*!< Internal SRAM queue for Mandris occupied by more than 3 request.
++	 * the number of cycles when the ISRAM queue is full
++	 */
++	PXA3xx_EVENT_ISRAM_FULL,
++
++	/*!< the number of cycles when external memory controller bus
++	 * is occupied
++	 */
++	PXA3xx_EVENT_EXMEM,
++
++	/*!< the number of cycles when external data flash bus is occupies */
++	PXA3xx_EVENT_DFC,
++
++	/*!< Core write request count*/
++	PXA3xx_EVENT_CORE_WRITE=PXA3xx_EVENT_MASK|36,
++
++	/*!< DMA write request count*/
++	PXA3xx_EVENT_DMA_WRITE,
++
++	/*!< Camera interface write request cout*/
++	PXA3xx_EVENT_CAMERA_WRITE,
++
++	/*!< USB 2.0 write request count*/
++	PXA3xx_EVENT_USB20_WRITE,
++
++	/*!< 2D grahpic write request count*/
++	PXA3xx_EVENT_2D_WRITE,
++
++	/*!< USB1.1 host write reqeust count*/
++	PXA3xx_EVENT_USB11_WRITE,
++
++	/*!< PX1 bus reqeust. length of time that at least one bus request
++	 * is asserted on PX bus 1
++	 */
++	PXA3xx_EVENT_PX1_REQUEST,
++
++	/*!< PX2 bus reqeust. length of time that at least one bus request
++	 * is asserted on PX bus 2
++	 */
++	PXA3xx_EVENT_PX2_REQUEST,
++
++	/*!< PX1 bus retries. number of retries on PX bus 1*/
++	PXA3xx_EVENT_PX1_RETRIES,
++
++	/*!< PX2 bus retries. number of retries on PX bus 2*/
++	PXA3xx_EVENT_PX2_RETRIES,
++
++	/*!< Temperature leve 1. time the part has spent in temperature range 1*/
++	PXA3xx_EVENT_TEMPERATURE_1,
++
++	/*!< Temperature leve 1. time the part has spent in temperature range 2*/
++	PXA3xx_EVENT_TEMPERATURE_2,
++
++	/*!< Temperature leve 1. time the part has spent in temperature range 3*/
++	PXA3xx_EVENT_TEMPERATURE_3,
++
++	/*!< Temperature leve 1. time the part has spent in temperature range 4*/
++	PXA3xx_EVENT_TEMPERATURE_4,
++
++	/*!< Core read/write latency measurement. amount of time when core
++	 * have more than 1 read/write request outstanding
++	 */
++	PXA3xx_EVENT_CORE_LATENCY_1,
++
++	/*!< Core read/write latency measurement. amount of time when core
++	 * have more than 2 read/write request outstanding
++	 */
++	PXA3xx_EVENT_CORE_LATENCY_2,
++
++	/*!< Core read/write latency measurement. amount of time when core
++	 * have more than 3 read/write request outstanding
++	 */
++	PXA3xx_EVENT_CORE_LATENCY_3,
++
++	/*!< Core read/write latency measurement. amount of time when core
++	 * have more than 4 read/write request outstanding
++	 */
++	PXA3xx_EVENT_CORE_LATENCY_4,
++
++	/*!<  PX1 to IM read/write latency measurement. Amount of time when
++	 * PX1 to IM has more than 1 read/write requests outstanding.
++	 */
++	PXA3xx_EVENT_PX1_IM_1,
++
++	/*!<  PX1 to IM read/write latency measurement. Amount of time when
++	 * PX1 to IM has more than 2 read/write requests outstanding.
++	 */
++	PXA3xx_EVENT_PX1_IM_2,
++
++	/*!<  PX1 to IM read/write latency measurement. Amount of time when
++	 * PX1 to IM has more than 3 read/write requests outstanding.
++	 */
++	PXA3xx_EVENT_PX1_IM_3,
++
++	/*!<  PX1 to IM read/write latency measurement. Amount of time when
++	 * PX1 to IM has more than 4 read/write requests outstanding.
++	 */
++	PXA3xx_EVENT_PX1_IM_4,
++
++	/*!<  PX1 to DMEM/SMEM read/write latency measurement. Amount of time
++	 * when PX1 to DMEM/SMEM has more than 1 read/write requests outstanding.
++	 */
++	PXA3xx_EVENT_PX1_MEM_1,
++
++	/*!<  PX1 to DMEM/SMEM read/write latency measurement. Amount of time
++	 * when PX1 to DMEM/SMEM has more than 2 read/write requests outstanding.
++	 */
++	PXA3xx_EVENT_PX1_MEM_2,
++
++	/*!<  PX1 to DMEM/SMEM read/write latency measurement. Amount of time
++	 * when PX1 to DMEM/SMEM has more than 3 read/write requests outstanding.
++	 */
++
++	PXA3xx_EVENT_PX1_MEM_3,
++	/*!<  PX1 to DMEM/SMEM read/write latency measurement. Amount of time
++	 * when PX1 to DMEM/SMEM has more than 4 read/write requests outstanding.
++	 */
++	PXA3xx_EVENT_PX1_MEM_4,
++
++	/*!<  PX2 to IM read/write latency measurement. Amount of time when
++	 * PX2 to IM has more than 1 read/write requests outstanding.
++	 */
++	PXA3xx_EVENT_PX2_IM_1,
++
++	/*!<  PX2 to IM read/write latency measurement. Amount of time when
++	 * PX2 to IM has more than 2 read/write requests outstanding.
++	 */
++	PXA3xx_EVENT_PX2_IM_2,
++
++	/*!<  PX2 to IM read/write latency measurement. Amount of time when
++	 * PX2 to IM has more than 3 read/write requests outstanding.
++	 */
++	PXA3xx_EVENT_PX2_IM_3,
++
++	/*!<  PX2 to IM read/write latency measurement. Amount of time when
++	 * PX2 to IM has more than 4 read/write requests outstanding.
++	 */
++	PXA3xx_EVENT_PX2_IM_4,
++
++	/*!<  PX2 to DMEM/SMEM read/write latency measurement. Amount of time
++	 * when PX2 to DMEM/SMEM has more than 1 read/write requests outstanding.
++	 */
++	PXA3xx_EVENT_PX2_MEM_1,
++
++	/*!<  PX2 to DMEM/SMEM read/write latency measurement. Amount of time
++	 * when PX2 to DMEM/SMEM has more than 2 read/write requests outstanding.
++	 */
++	PXA3xx_EVENT_PX2_MEM_2,
++
++	/*!<  PX2 to DMEM/SMEM read/write latency measurement. Amount of time
++	 * when PX2 to DMEM/SMEM has more than 3 read/write requests outstanding.
++	 */
++	PXA3xx_EVENT_PX2_MEM_3,
++
++	/*!<  PX2 to DMEM/SMEM read/write latency measurement. Amount of time
++	 * when PX2 to DMEM/SMEM has more than 4 read/write requests outstanding.
++	 */
++	PXA3xx_EVENT_PX2_MEM_4
++};
++
++#ifdef __KERNEL__
++struct pxa3xx_pmu_info {
++	/* performance monitor unit register base */
++	unsigned char __iomem	*pmu_base;
++};
++
++#ifdef __cplusplus
++extern "C"
++{
++#endif
++
++/*
++ * This routine reads the designated PMU register via CoProcessor 14
++ *
++ * @param   aReg	PMU register number to read define in int
++ * @return  		32-bit value read from register
++ */
++extern unsigned int pmu_read_reg(unsigned int aReg);
++
++/*
++ * This routine Writes the designated PMU register via CoProcessor 14
++ *
++ * @param   aReg	PMU register number to read define in int
++ *          aValue	Value to write to PMU register
++ * @return
++ */
++extern void pmu_write_reg(unsigned int aReg, unsigned int aValue);
++
++extern int pmu_select_event(int counter, int type);
++
++extern void pxa3xx_set_pmu_info(void *info);
++
++#ifdef __cplusplus
++}
++#endif
++#endif
++
++#endif //__PMU_H__
++
+diff -ur linux-2.6.32/arch/arm/mach-pxa/include/mach/prm.h kernel/arch/arm/mach-pxa/include/mach/prm.h
+--- linux-2.6.32/arch/arm/mach-pxa/include/mach/prm.h	2009-12-13 12:59:30.199033933 +0200
++++ kernel/arch/arm/mach-pxa/include/mach/prm.h	2009-12-12 16:09:26.459612243 +0200
+@@ -0,0 +1,138 @@
++/*
++ * include/asm-arm/arch-pxa/prm.h
++ *
++ * Copyright (C) 2006, Intel Corporation.
++ *
++ * This program is free software; you can redistribute it and/or modify
++ * it under the terms of the GNU General Public License version 2 as
++ * published by the Free Software Foundation.
++ */
++
++#ifndef __PRM_H
++#define __PRM_H
++
++#include <linux/interrupt.h>
++#include <mach/irqs.h>
++#include <mach/pmu.h>
++#include <mach/pxa3xx_dvfm.h>
++
++#define MAX_GROUPS 	2
++#define MAX_CLIENTS	16
++
++typedef enum {
++	/* tag the loweset priority*/
++	PRI_LOWEST = 0,
++	/*define the possible priorities here*/
++	PRI_IPMC = PRI_LOWEST,
++	PRI_PROFILER,
++	PRI_VTUNE,
++	/*tag the highest priority*/
++	MAX_PRIORITIES,
++	PRI_HIGHEST = MAX_PRIORITIES - 1,
++} prm_priority;
++
++struct prm_group;
++struct prm_resource;
++struct prm_resource_state;
++
++typedef enum {
++	PRM_RES_APPROPRIATED,
++	PRM_RES_READY,
++} prm_event;
++
++typedef enum {
++	PRM_CCNT = 0,
++	PRM_PMN0,
++	PRM_PMN1,
++	PRM_PMN2,
++	PRM_PMN3,
++	PRM_VCC0,
++	PRM_VCC1,
++	PRM_IDLE_PROFILER,
++	PRM_COP,
++	RESOURCE_NUM,
++} prm_resource_id;
++
++typedef void (*clientcallback)(prm_event, unsigned int, void *);
++
++/* The gourp includes a set of resources. If one of the set of resources is
++ * appropriated, the other resources will not available for access. But the
++ * resources are still allocated by the client. So the group is defined as
++ * a set of resources that all can be accessed or all can not be accessed.
++ */
++struct prm_group {
++	unsigned int id;
++	/* appropriated resources count */
++	unsigned int appropriated_cnt;
++	/* total resources count in the group */
++	unsigned int member_cnt;
++	/* list for all the resources in the group */
++	struct list_head resources;
++	struct proc_dir_entry *dir;
++};
++
++struct prm_client {
++	/* client id */
++	unsigned int id;
++	/* process id for the client */
++	unsigned int pid;
++	/* priority for the client.(LOW or HIGH) */
++	prm_priority priority;
++	/* name of the client */
++	char *name;
++	/* How many groups in the client */
++	unsigned int group_cnt;
++	/* support MAXGROUP groups, some may be NULL */
++	struct prm_group *groups[MAX_GROUPS];
++	void *client_data;
++	/* notifier for resource appropriate and ready */
++	clientcallback notify;
++	irq_handler_t handler;
++	void *dev_id;
++	struct proc_dir_entry *dir;
++};
++
++struct prm_resource_state {
++	/* which client allocate the resources. In every priority,
++	 * there can be only one client allocate the resource
++	 */
++	struct prm_client *allocate;
++	/* which group it belongs to */
++	struct prm_group *group;
++	int active;
++	struct prm_resource *resource;
++	/* used by prm_group->resources for link the resources into the group */
++	struct list_head entry;
++	struct proc_dir_entry *dir;
++};
++
++struct prm_resource {
++	struct prm_client *access;	/* Only one client can access it */
++	prm_resource_id id;
++	struct prm_resource_state priority[MAX_PRIORITIES];
++	struct proc_dir_entry *dir;
++};
++
++int prm_open_session(prm_priority , char * , clientcallback , void * );
++int prm_close_session(unsigned int );
++int prm_allocate_resource(unsigned int , prm_resource_id , unsigned int );
++int prm_free_resources(unsigned int , unsigned int );
++int prm_commit_resources(unsigned int , unsigned int );
++int pmu_read_register(unsigned int , int , unsigned int * );
++int pmu_write_register(unsigned int , int , unsigned int );
++int pmu_set_event(unsigned int , unsigned int , int * , int );
++int pmu_enable_event_counting(unsigned int );
++int pmu_disable_event_counting(unsigned int );
++int pmu_enable_event_interrupt(unsigned int , int );
++int pmu_disable_event_interrupt(unsigned int , int );
++int pmu_register_isr(unsigned int , irq_handler_t, void * );
++int pmu_unregister_isr(unsigned int );
++int cop_get_num_of_cops(void);
++int cop_get_cop(unsigned int , unsigned int , struct pxa3xx_fv_info *);
++int cop_set_cop(unsigned int , unsigned int , int mode);
++int cop_get_def_cop(unsigned int , unsigned int *, struct pxa3xx_fv_info *);
++int cop_set_def_cop(unsigned int );
++int cop_get_cur_cop(unsigned int , unsigned int *, struct pxa3xx_fv_info *);
++
++#endif
++
+diff -ur linux-2.6.32/arch/arm/mach-pxa/include/mach/pxa3xx_dvfm.h kernel/arch/arm/mach-pxa/include/mach/pxa3xx_dvfm.h
+--- linux-2.6.32/arch/arm/mach-pxa/include/mach/pxa3xx_dvfm.h	2009-12-13 12:59:37.209033179 +0200
++++ kernel/arch/arm/mach-pxa/include/mach/pxa3xx_dvfm.h	2009-12-12 16:09:26.462949527 +0200
+@@ -0,0 +1,94 @@
++/*
++ * This software program is licensed subject to the GNU General Public License
++ * (GPL).Version 2,June 1991, available at http://www.fsf.org/copyleft/gpl.html
++
++ * (C) Copyright 2007 Marvell International Ltd.
++ * All Rights Reserved
++ */
++
++#ifndef PXA3XX_DVFM_H
++#define PXA3XX_DVFM_H
++
++#include <mach/dvfm.h>
++#include <mach/pxa3xx_pm.h>
++
++#define DMEMC_FREQ_HIGH		0
++#define DMEMC_FREQ_LOW		1
++#define DMEMC_D0CS_ENTER	2
++#define DMEMC_D0CS_EXIT		3
++
++#define OP_NAME_LEN		16
++
++enum {
++	POWER_MODE_D0 = 0,
++	POWER_MODE_D0CS,
++	POWER_MODE_D1,
++	POWER_MODE_D2,
++	POWER_MODE_CG,
++};
++
++enum {
++	OP_FLAG_FACTORY = 0,
++	OP_FLAG_USER_DEFINED,
++	OP_FLAG_BOOT,
++	OP_FLAG_ALL,
++};
++
++enum {
++	IDLE_D0 = 0,
++	IDLE_D0CS = 1,
++	IDLE_D1 = 2,
++	IDLE_D2 = 4,
++	IDLE_CG = 8,
++};
++
++struct dvfm_md_opt {
++	int	vcc_core;
++	int	vcc_sram;
++	int	xl;
++	int	xn;
++	int	core;
++	int	smcfs;
++	int	sflfs;
++	int	hss;
++	int	dmcfs;
++	int	df_clk;
++	int	empi_clk;
++	int	power_mode;
++	int	flag;
++	int	lpj;
++	char	name[OP_NAME_LEN];
++};
++
++/* This structure is similar to dvfm_md_opt.
++ * Reserve this structure in order to keep compatible
++ */
++struct pxa3xx_fv_info {
++	unsigned long xl;
++	unsigned long xn;
++	unsigned int vcc_core;
++	unsigned int vcc_sram;
++	unsigned long smcfs;
++	unsigned long sflfs;
++	unsigned long hss;
++	unsigned long dmcfs;
++	unsigned long df_clk;
++	unsigned long empi_clk;
++	unsigned long d0cs;
++    /* WARNING: above fields must be consistent with PM_FV_INFO!!!*/
++
++	unsigned long lpj; /* New value for loops_per_jiffy */
++};
++
++struct pxa3xx_freq_mach_info {
++	int flags;
++};
++
++#define PXA3xx_USE_POWER_I2C  (1UL << 0)
++extern void set_pxa3xx_freq_info(struct pxa3xx_freq_mach_info *info);
++extern void set_pxa3xx_freq_parent(struct device *parent_dev);
++
++extern int md2fvinfo(struct pxa3xx_fv_info *fv_info, struct dvfm_md_opt *orig);
++
++#endif
++
+diff -ur linux-2.6.32/arch/arm/mach-pxa/include/mach/pxa3xx_pm.h kernel/arch/arm/mach-pxa/include/mach/pxa3xx_pm.h
+--- linux-2.6.32/arch/arm/mach-pxa/include/mach/pxa3xx_pm.h	2009-12-13 12:59:45.791952709 +0200
++++ kernel/arch/arm/mach-pxa/include/mach/pxa3xx_pm.h	2009-12-12 16:09:26.462949527 +0200
+@@ -0,0 +1,530 @@
++/*
++ * Monahans Power Management Routines
++ *
++ * Copyright (C) 2004, Intel Corporation(chao.xie@intel.com).
++ *
++ * This software program is licensed subject to the GNU General Public License
++ * (GPL).Version 2,June 1991, available at http://www.fsf.org/copyleft/gpl.html
++ *
++ *(C) Copyright 2006 Marvell International Ltd.
++ * All Rights Reserved
++ */
++
++#ifndef __PXA3xx_PM_H__
++#define __PXA3xx_PM_H__
++
++#include <asm/types.h>
++
++/* clock manager registers */
++#define ACCR_OFF		0x00
++#define ACSR_OFF		0x04
++#define AICSR_OFF		0x08
++#define D0CKEN_A_OFF		0x0c
++#define D0CKEN_B_OFF		0x10
++#define AC97_DIV_OFF		0x14
++#define OSCC_OFF		0x10000
++
++/* service power management uinit */
++#define PSR_OFF			0x004
++#define PSPR_OFF		0x008
++#define PCFR_OFF		0x00C
++#define PWER_OFF		0x010
++#define PWSR_OFF		0x014
++#define PECR_OFF		0x018
++#define CSER_OFF		0x01C
++#define DCDCSR_OFF		0x080
++#define AVCR_OFF		0x094
++#define SVCR_OFF		0x098
++#define CVCR_OFF		0x09C
++#define PSBR_OFF		0x0A0
++#define PVCR_OFF		0x100
++#if defined(CONFIG_CPU_PXA935)
++#define SDCR_OFF		0x08C
++#endif
++
++/* slave power management unit */
++#define ASCR_OFF		0x00
++#define ARSR_OFF		0x04
++#define AD3ER_OFF		0x08
++#define AD3SR_OFF		0x0c
++#define AD2D0ER_OFF		0x10
++#define AD2D0SR_OFF		0x14
++#define AD2D1ER_OFF		0x18
++#define AD2D1SR_OFF		0x1c
++#define AD1D0ER_OFF		0x20
++#define AD1D0SR_OFF		0x24
++#define ASDCNT_OFF		0x28
++#define AGENP_OFF		0x2c
++#define AD3R_OFF		0x30
++#define AD2R_OFF		0x34
++#define AD1R_OFF		0x38
++
++/* dynamic memory controller registers */
++#define MDCNFG_OFF		0x0000
++#define MDREFR_OFF		0x0004
++#define FLYCNFG_OFF		0x0020
++#define MDMRS_OFF		0x0040
++#define DDR_SCAL_OFF		0x0050
++#define DDR_HCAL_OFF		0x0060
++#define DDR_WCAL_OFF		0x0068
++#define DMCIER_OFF		0x0070
++#define DMCISR_OFF		0x0078
++#define DMCISR2_OFF		0x007C
++#define DDR_DLS_OFF		0x0080
++#define EMPI_OFF		0x0090
++#define RCOMP_OFF		0x0100
++#define PAD_MA_OFF		0x0110
++#define PAD_MDMSB_OFF		0x0114
++#define PAD_MDLSB_OFF		0x0118
++#define PAD_SDRAM_OFF		0x011C
++#define PAD_SDCLK_OFF		0x0120
++#define PAD_SDCS_OFF		0x0124
++#define PAD_SMEM_OFF		0x0128
++#define PAD_SCLK_OFF		0x012C
++
++/* static memory controller registers */
++#define MSC0_OFF		0x0008
++#define MSC1_OFF		0x000C
++#define MECR_OFF		0x0014
++#define SXCNFG_OFF		0x001C
++#define MCMEM0_OFF		0x0028
++#define MCATT0_OFF		0x0030
++#define MCIO0_OFF		0x0038
++#define MEMCLKCFG_OFF		0x0068
++#define CSADRCFG0_OFF		0x0080
++#define CSADRCFG1_OFF		0x0084
++#define CSADRCFG2_OFF		0x0088
++#define CSADRCFG3_OFF		0x008C
++#define CSADRCFG_P_OFF		0x0090
++#define CSMSADRCFG_OFF		0x00A0
++
++/* OS Timer address space */
++#define OST_START		0x40a00000
++#define OST_END			0x40a000df
++
++/* System Bus Arbiter address space */
++#define ARB_START		0x4600fe00
++#define ARB_END			0x4600fe07
++
++/* Registers offset within ARB space */
++#define ARBCTL1_OFF		0x0000
++#define ARBCTL2_OFF		0x0004
++
++/* Dynamic memory controll address space */
++#define DMC_START		0x48100000
++#define DMC_END			0x48100fff
++
++/* static memory controll address space */
++#define SMC_START		0x4a000000
++#define SMC_END			0x4a0000ff
++
++/* Power Management Unit address space */
++#define PM_START		0x40f50000
++#define PM_END			0x40f5018f
++
++/* Bits definition for Clock Control Register */
++#define ACCR_PCCE	(1 << 11)
++
++#define ACSR_XPLCK	(1 << 29)
++#define ACSR_SPLCK	(1 << 28)
++
++#define AICSR_PCIE	(1 << 4)
++#define AICSR_TCIE	(1 << 2)
++#define AICSR_FCIE	(1 << 0)
++
++/* Bits definition for RTC Register */
++#define RTSR_PICE	(1 << 15)
++#define RTSR_PIALE	(1 << 14)
++
++/* Bits definition for Power Control Register */
++#define ASCR_RDH	(1 << 31)
++#define ASCR_D1S	(1 << 2)
++#define ASCR_D2S	(1 << 1)
++#define ASCR_D3S	(1 << 0)
++#define ASCR_MASK	(ASCR_D1S | ASCR_D2S | ASCR_D3S)
++#define PSR_MASK	0x07
++#define PCFR_L1DIS	(1 << 13)
++#define PCFR_L0EN	(1 << 12)
++#define PECR_E1IS	(1 << 31)
++#define PECR_E1IE	(1 << 30)
++#define PECR_E0IS	(1 << 29)
++#define PECR_E0IE	(1 << 28)
++#define PECR_DIR1	(1 << 5)
++#define PECR_DIR0	(1 << 4)
++
++/* Bits definition for Oscillator Configuration Register */
++#define OSCC_GPRM		(1 << 18)	/* GB PLL Request Mask */
++#define OSCC_GPLS		(1 << 17)	/* GB PLL Lock Status */
++
++/* Bits definition for Application Subsystem General Purpose Register */
++#define AGENP_GBPLL_CTRL	(1 << 29)
++#define AGENP_GBPLL_DATA	(1 << 28)	/* Turn on/off GB PLL */
++#define AGENP_SAVE_WK		(1 << 2)	/* Save wakeup */
++
++/* Registers offset within ARB space */
++#define BPB_START		0x42300000
++#define BPB_END			0x4230004B
++
++/* GPIO Wakeup Status Register */
++#define GWSR(x)			((x << 2) + 0x38)
++#define GWSR1			0x3C
++#define GWSR2			0x40
++#define GWSR3			0x44
++#define GWSR4			0x48
++
++/* bits definitions */
++#define ASCR_MTS_OFFSET		12
++#define ASCR_MTS_S_OFFSET	8
++
++#define ACSR_SPLCK_OFFSET    28
++#define ACSR_XPLCK_OFFSET    29
++
++#define ACCR_XL_OFFSET		0
++#define ACCR_XN_OFFSET		8
++#define ACCR_DMCFS_OFFSET	12
++#define ACCR_HSS_OFFSET		14
++#define ACCR_XSPCLK_OFFSET	16
++#define ACCR_SFLFS_OFFSET	18
++#define ACCR_SMCFS_OFFSET	23
++#define ACCR_D0CS_OFFSET	26
++#define ACCR_VAUFS_OFFSET	28
++#define ACCR_SPDIS_OFFSET	30
++#define ACCR_XPDIS_OFFSET	31
++
++#define ACSR_VAUFS_OFFSET	21
++
++#define ACSR_VAUFS_MASK		(0x03 << ACSR_VAUFS_OFFSET)
++
++#define MEMCLKCFG_EMPI_OFFSET	0
++#define MEMCLKCFG_DF_OFFSET	16
++
++#define HCAL_HCEN_OFFSET	31
++
++#define MDCNFG_HWNOPHD_OFFSET	28
++#define MDCNFG_HWFREQ_OFFSET	29
++#define MDCNFG_DMCEN_OFFSET	30
++#define MDCNFG_DMAP_OFFSET	31
++
++/* mode save flags */
++#define PM_MODE_SAVE_FLAG_SYS	0x1
++#define PM_MODE_SAVE_FLAG_IRQ	0x2
++#define PM_MODE_SAVE_FLAG_FIQ	0x4
++#define PM_MODE_SAVE_FLAG_ABT	0x8
++#define PM_MODE_SAVE_FLAG_UND	0x10
++#define PM_MODE_SAVE_FLAG_SVC	0x20
++
++/* value for PWRMODE register */
++#define PXA3xx_PM_S2D3C4	0x06
++#define PXA3xx_PM_S0D2C2	0x03
++#define PXA3xx_PM_S3D4C4	0x07
++#define PXA3xx_PM_S0D1C2	0x02
++#define PXA3xx_PM_S0D0C1	0x01
++
++/* CPSR Processor constants */
++#define CPSR_Mode_MASK		(0x0000001F)
++#define CPSR_Mode_USR		(0x10)
++#define CPSR_Mode_FIQ		(0x11)
++#define CPSR_Mode_IRQ		(0x12)
++#define CPSR_Mode_SVC		(0x13)
++#define CPSR_Mode_ABT		(0x17)
++#define CPSR_Mode_UND		(0x1B)
++#define CPSR_Mode_SYS		(0x1F)
++#define CPSR_I_Bit		(0x80)
++#define CPSR_F_Bit		(0x40)
++
++
++/****************************************************************************/
++#define PXA3xx_PM_WE_EXTERNAL0	(0x1UL << 0)
++#define PXA3xx_PM_WE_EXTERNAL1	(0x1UL << 1)
++#define PXA3xx_PM_WE_GENERIC(x) (0x1UL << (x + 2))
++#define PXA3xx_PM_WE_DKEY	(0x1UL << 2)
++#define PXA3xx_PM_WE_DKEY1	(0x1UL << 3)
++#define PXA3xx_PM_WE_BTUART	(0x1UL << 4)
++#define PXA3xx_PM_WE_PMIC	(0x1UL << 5)
++#define PXA3xx_PM_WE_NDINT	(0x1UL << 6)
++#define PXA3xx_PM_WE_MMC1	(0x1UL << 7)
++#define PXA3xx_PM_WE_MMC2	(0x1UL << 8)
++#define PXA3xx_PM_WE_SSP	(0x1UL << 9)
++#define PXA3xx_PM_WE_SSP4	(0x1UL << 10)
++#define PXA3xx_PM_WE_UART1	(0x1UL << 11)
++#define PXA3xx_PM_WE_CI2C	(0x1UL << 12)
++#define PXA3xx_PM_WE_SSP2	(0x1UL << 13)
++#define PXA3xx_PM_WE_WDT	(0x1UL << 14)
++#define PXA3xx_PM_WE_GPIO	(0x1UL << 15)
++#define PXA3xx_PM_WE_OTG	(0x1UL << 16)
++#define PXA3xx_PM_WE_INTC	(0x1UL << 17)
++#define PXA3xx_PM_WE_MLCD	(0x1UL << 18)
++#define PXA3xx_PM_WE_USIM0	(0x1UL << 19)
++#define PXA3xx_PM_WE_USIM1	(0x1UL << 20)
++#define PXA3xx_PM_WE_MKEY	(0x1UL << 21)
++#define PXA3xx_PM_WE_MUX2	(0x1UL << 22)
++#define PXA3xx_PM_WE_MUX3	(0x1UL << 23)
++#define PXA3xx_PM_WE_MSL0	(0x1UL << 24)
++#define PXA3xx_PM_WE_RESERVE1	(0x1UL << 25)
++#define PXA3xx_PM_WE_USB2	(0x1UL << 26)
++#define PXA3xx_PM_WE_DMC	(0x1UL << 27)
++#define PXA3xx_PM_WE_USBH	(0x1UL << 28)
++#define PXA3xx_PM_WE_TSI	(0x1UL << 29)
++#define PXA3xx_PM_WE_OST	(0x1UL << 30)
++#define PXA3xx_PM_WE_RTC 	(0x1UL << 31)
++
++
++#define PWSR_EDR0		(0x1 << 0)
++#define PWSR_EDR1		(0x1 << 1)
++#define PWSR_EDF0		(0x1 << 2)
++#define PWSR_EDF1		(0x1 << 3)
++#define PWSR_EERTC		(0x1 << 31)
++
++#define PWER_WER0		(0x1 << 0)
++#define PWER_WER1		(0x1 << 1)
++#define PWER_WEF0		(0x1 << 2)
++#define PWER_WEF1		(0x1 << 3)
++#define PWER_WERTC		(0x1 << 31)
++
++#define WORD_SIZE 4
++
++/* the position of each data memeber */
++#define SleepState_begin		0x0
++#define SleepState_checksum		0x0
++#define SleepState_wordCount		(SleepState_checksum + WORD_SIZE)
++#define SleepState_areaAddress		(SleepState_wordCount + WORD_SIZE)
++#define SleepState_modeSaveFlags	(SleepState_areaAddress + WORD_SIZE)
++
++/* save ARM registers */
++#define SleepState_ENTRY_REGS		(SleepState_modeSaveFlags + WORD_SIZE)
++#define SleepState_ENTRY_CPSR		(SleepState_ENTRY_REGS)
++#define SleepState_ENTRY_SPSR		(SleepState_ENTRY_CPSR + WORD_SIZE)
++#define SleepState_ENTRY_R0		(SleepState_ENTRY_SPSR + WORD_SIZE)
++#define SleepState_ENTRY_R1		(SleepState_ENTRY_R0 + WORD_SIZE)
++#define SleepState_SYS_REGS		(SleepState_ENTRY_REGS + 17*WORD_SIZE)
++#define SleepState_FIQ_REGS		(SleepState_SYS_REGS + 2*WORD_SIZE)
++#define SleepState_IRQ_REGS		(SleepState_FIQ_REGS + 8*WORD_SIZE)
++#define SleepState_ABT_REGS		(SleepState_IRQ_REGS + 3*WORD_SIZE)
++#define SleepState_UND_REGS		(SleepState_ABT_REGS + 3*WORD_SIZE)
++#define SleepState_SVC_REGS		(SleepState_UND_REGS + 3*WORD_SIZE)
++
++/* save MMU settings */
++#define SleepState_Cp15_ACR_MMU		(SleepState_SVC_REGS + 3*WORD_SIZE)
++#define SleepState_Cp15_AUXCR_MMU	(SleepState_Cp15_ACR_MMU + WORD_SIZE)
++#define SleepState_Cp15_TTBR_MMU	(SleepState_Cp15_AUXCR_MMU + WORD_SIZE)
++#define SleepState_Cp15_DACR_MMU	(SleepState_Cp15_TTBR_MMU + WORD_SIZE)
++#define SleepState_Cp15_PID_MMU		(SleepState_Cp15_DACR_MMU + WORD_SIZE)
++#define SleepState_Cp15_CPAR		(SleepState_Cp15_PID_MMU + WORD_SIZE)
++
++#define SleepState_extendedChecksumByteCount	(SleepState_Cp15_CPAR + WORD_SIZE)
++#define SleepState_psprAddress		(SleepState_extendedChecksumByteCount + WORD_SIZE)
++#define SleepState_flushFunc		(SleepState_psprAddress + WORD_SIZE)
++#define SleepState_end			(SleepState_flushFunc + WORD_SIZE)
++#define SleepState_size			(SleepState_end - SleepState_begin)
++
++#ifndef __ASSEMBLY__
++
++typedef struct {
++	unsigned long value;
++	struct {
++		unsigned ext0 : 1;
++		unsigned ext1 : 1;
++		unsigned uart1 : 1;
++		unsigned uart2 : 1;
++		unsigned uart3 : 1;
++		unsigned wifi : 1;      /* wifi use UART1's pin as wakeup source */
++		unsigned mmc1_cd : 1;
++		unsigned mmc2_cd : 1;
++		unsigned mmc3_cd : 1;
++		unsigned mmc1_dat1 : 1;
++		unsigned mmc2_dat1 : 1;
++		unsigned mmc3_dat1 : 1;
++		unsigned mkey : 1;
++		unsigned usbotg : 1;
++		unsigned mlcd : 1;
++		unsigned dkey : 1;
++		unsigned usb2 : 1;      /* USB 2.0 client */
++		unsigned usbh : 1;      /* USB Host Port 1 */
++		unsigned msl : 1;
++		unsigned tsi : 1;
++		unsigned ost : 1;
++		unsigned rtc : 1;
++		unsigned eth : 1;
++		unsigned onkey : 1;	/* pmic wakeup resources */
++		unsigned usbc : 1;	/* USB client for cable and charger */
++		unsigned bat_full : 1;	/* battery full */
++		unsigned bat_low : 1;	/* battery low */
++		unsigned bt : 1;	/* bluetooth use STRXD pin */
++		unsigned cmwdt : 1;
++		unsigned psensor : 1;  /* psensor */
++	} bits;
++} pm_wakeup_src_t;
++
++
++#ifdef __KERNEL__
++struct intc_regs {
++	unsigned int		iccr;
++	unsigned int		ipr[32];
++	unsigned int		ipr2[21];
++	unsigned int		icmr;
++	unsigned int		icmr2;
++	unsigned int		iclr;
++	unsigned int		iclr2;
++};
++
++struct clock_regs {
++	unsigned int		aicsr;
++	unsigned int		ckena;
++	unsigned int		ckenb;
++	unsigned int		oscc;
++};
++
++struct ost_regs {
++	unsigned int		ossr;
++	unsigned int		oier;
++	unsigned int		oscr;
++	unsigned int		oscr4;
++	unsigned int		osmr4;
++	unsigned int		omcr4;
++};
++
++struct rtc_regs {
++	unsigned int		rtsr;
++	unsigned int		piar;
++};
++
++struct smc_regs {
++	unsigned char __iomem	*membase;
++	unsigned int		msc0;
++	unsigned int		msc1;
++	unsigned int		mecr;
++	unsigned int		sxcnfg;
++	unsigned int		mcmem0;
++	unsigned int		mcatt0;
++	unsigned int		mcio0;
++	unsigned int		memclkcfg;
++	unsigned int		cscfg0;
++	unsigned int		cscfg1;
++	unsigned int		cscfg2;
++	unsigned int		cscfg3;
++	unsigned int		cscfg_p;
++	unsigned int		csmscfg;
++};
++
++struct arb_regs {
++	unsigned char __iomem	*membase;
++	unsigned int		ctl1;
++	unsigned int		ctl2;
++};
++
++struct pmu_regs {
++	unsigned int		pcfr;
++	unsigned int		pecr;
++	unsigned int		pvcr;
++};
++
++#define MAX_MFP_PINS 419
++
++struct mfp_regs {
++	unsigned int		mfp[MAX_MFP_PINS];
++};
++
++struct gpio_regs {
++	unsigned int		gplr0;
++	unsigned int		gplr1;
++	unsigned int		gplr2;
++	unsigned int		gplr3;
++	unsigned int		gpdr0;
++	unsigned int		gpdr1;
++	unsigned int		gpdr2;
++	unsigned int		gpdr3;
++	unsigned int		grer0;
++	unsigned int		grer1;
++	unsigned int		grer2;
++	unsigned int		grer3;
++	unsigned int		gfer0;
++	unsigned int		gfer1;
++	unsigned int		gfer2;
++	unsigned int		gfer3;
++};
++
++struct pm_save_data {
++	u32 checksum;
++	u32 wordCount;
++	u32 areaAddress;
++	u32 modeSaveFlags;
++	/* current mode registers cpsr, sprsr, r0-r12, lr, sp */
++	u32 ENTRY_REGS[17];
++	/* SYS mode registers:sp, lr */
++	u32 SYS_REGS[2];
++	/* FIQ mode registers:spsr, r8-r12, sp, lr */
++	u32 FIQ_REGS[8];
++	/* IRQ mode registers:spsr, sp, lr */
++	u32 IRQ_REGS[3];
++	/* ABT mode registers:spsr, sp, lr */
++	u32 ABT_REGS[3];
++	/* UND mode registers:spsr, sp, lr */
++	u32 UND_REGS[3];
++	/* SVC mode registers:spsr, sp, lr */
++	u32 SVC_REGS[3];
++	/* MMU registers */
++	u32 CP15_ACR_MMU;
++	u32 CP15_AUXCR_MMU;
++	u32 CP15_TTBR_MMU;
++	u32 CP15_DACR_MMU;
++	u32 CP15_PID_MMU;
++	u32 CP15_CPAR;
++
++	u32 extendedChecksumByteCount;
++	u32 psprAddress;
++	void (*flushFunc)(void);
++	/* the parameter is the reserved bytes from 0x5c010000 */
++	/* It returns the physical address of initlization code in SRAM */
++};
++
++struct pxa3xx_pm_regs {
++	/* It's used to save core registers. */
++	struct pm_save_data	pm_data;
++	struct mfp_regs		mfp;
++	struct gpio_regs	gpio;
++	struct intc_regs	intc;
++	struct clock_regs	clock;
++	struct ost_regs		ost;
++	struct rtc_regs		rtc;
++	struct smc_regs		smc;
++	struct arb_regs		arb;
++	struct pmu_regs		pmu;
++	/* It's the virtual address of ISRAM that can be accessed by kernel.
++	 */
++	void *sram_map;
++	/* It's used to save ISRAM data. */
++	void *sram;
++	/* It's used to save OBM that loaded from NAND flash. */
++	void *obm;
++	/* It's the address of DDR that stores key information.
++	 * Two words are used from the address.
++	 */
++	void *data_pool;
++	unsigned int word0;
++	unsigned int word1;
++};
++
++extern pm_wakeup_src_t wakeup_src;
++
++struct pxa3xx_peripheral_wakeup_ops {
++	int	(*init)(pm_wakeup_src_t *src);
++	int	(*query)(unsigned int reg, pm_wakeup_src_t *src);
++	int	(*ext)(pm_wakeup_src_t src, int enable);
++	int	(*key)(pm_wakeup_src_t src, int enable);
++	int	(*mmc)(pm_wakeup_src_t src, int enable);
++	int	(*uart)(pm_wakeup_src_t src, int enable);
++	int	(*eth)(pm_wakeup_src_t src, int enable);
++	int	(*tsi)(pm_wakeup_src_t src, int enable);
++	int	(*usb)(pm_wakeup_src_t src, int enable);
++	int	(*cmwdt)(pm_wakeup_src_t src, int enable);
++	int	(*psensor)(pm_wakeup_src_t src, int enable);
++};
++
++extern int pxa3xx_wakeup_register(struct pxa3xx_peripheral_wakeup_ops *);
++extern void pxa3xx_lock_suspend(void);
++extern void pxa3xx_unlock_suspend(void);
++extern void pxa3xx_lock_suspend_cancel(void);
++#endif
++#endif
++
++#endif
+diff -ur linux-2.6.32/arch/arm/mach-pxa/include/mach/pxa3xx_pmic.h kernel/arch/arm/mach-pxa/include/mach/pxa3xx_pmic.h
+--- linux-2.6.32/arch/arm/mach-pxa/include/mach/pxa3xx_pmic.h	2009-12-13 12:59:52.402368878 +0200
++++ kernel/arch/arm/mach-pxa/include/mach/pxa3xx_pmic.h	2009-12-12 16:09:26.462949527 +0200
+@@ -0,0 +1,194 @@
++#ifndef __PMIC_H__
++#define __PMIC_H__
++
++#include <linux/i2c.h>
++#include <linux/interrupt.h>
++
++/* this enum should be consistent with micco_power_module[]
++ * in arch/arm/mach-pxa/xxx(platform).c */
++enum {
++	/* Set command > 0xFFFF0000 to avoid wrong
++	 * parameter is used for pmic_set_voltage.
++	 */
++	VCC_CORE = 0xFFFF0000,
++	VCC_SRAM,
++	VCC_MVT,
++	VCC_3V_APPS,
++	VCC_SDIO,
++	VCC_CAMERA_ANA,
++	VCC_USB,
++	VCC_LCD,
++	VCC_TSI,
++	VCC_CAMERA_IO,
++	VCC_1P8V,
++	VCC_MEM,
++	HDMI_TX,
++	TECH_3V,
++	TECH_1P8V,
++
++	/* add your command here */
++	VCC_BT,
++	VCC_JOGBALL,
++	VCC_BTWIFFSHARE,
++	VCC_LCD_IO,
++	VCC_TOUCHKEY,
++	/* max command */
++	MAX_CMD,
++};
++
++enum {
++	LED_BACKLIGHT = 0xFFFF0000,
++	LED_VIBRATOR,
++	LED_FLASH,
++	LED_GREEN,
++	LED_RED,
++	LED_BLUE,
++	LED_KEYPAD,
++	LED_MAX_CMD,
++};	
++
++#define	PMIC_EVENT_EXTON	(1 << 0)
++#define	PMIC_EVENT_VBUS		(1 << 1)
++#define	PMIC_EVENT_USB		(PMIC_EVENT_EXTON | PMIC_EVENT_VBUS)
++
++#define	PMIC_EVENT_TOUCH	(1 << 2)
++
++#define PMIC_EVENT_OTGCP_IOVER	(1 << 3)
++
++#define	PMIC_EVENT_TBAT		(1 << 4)
++#define	PMIC_EVENT_REV_IOVER	(1 << 5)
++#define	PMIC_EVENT_IOVER	(1 << 6)
++#define	PMIC_EVENT_CHDET	(1 << 7)
++#define	PMIC_EVENT_VBATMON	(1 << 8)
++#define PMIC_EVENT_ONKEY 	(1 << 9)
++
++#ifdef CONFIG_MICCO_HEADSET_DETECT
++#define PMIC_EVENT_HEADSET      (1 << 10)
++#define PMIC_EVENT_HOOKSWITCH   (1 << 11)
++#endif
++#define	PMIC_EVENT_CH_CCTO	(1 << 12)
++#define	PMIC_EVENT_CH_TCTO	(1 << 13)
++
++#define	PMIC_EVENT_CHARGER	(PMIC_EVENT_TBAT |	\
++				 PMIC_EVENT_REV_IOVER |	\
++				 PMIC_EVENT_IOVER |	\
++				 PMIC_EVENT_CHDET |	\
++				 PMIC_EVENT_CH_CCTO |   \
++				 PMIC_EVENT_CH_TCTO |   \
++				 PMIC_EVENT_VBATMON)
++
++
++#define	PMIC_EVENT_USB_IN	(1 << 0)
++#define	PMIC_EVENT_AC_IN	(1 << 1)
++#define	PMIC_EVENT_CABLE_OUT	(1 << 2)
++#define PMIC_EVENT_CABLE_DETECT (PMIC_EVENT_USB_IN |	\
++				 PMIC_EVENT_AC_IN |	\
++				 PMIC_EVENT_CABLE_OUT)
++
++struct pmic_ops {
++	int (*get_voltage)(int cmd, int *pmv);
++	int (*set_voltage)(int cmd, int mv);
++	int (*enable_voltage)(int cmd, int enable);
++	int (*check_voltage)(int cmd);
++	int (*enable_led)(int cmd, int enable);
++	int (*enable_event)(unsigned long, int enable);
++	int (*is_vbus_assert)(void);
++	int (*is_avbusvld)(void);
++	int (*is_asessvld)(void);
++	int (*is_bsessvld)(void);
++	int (*is_srp_ready)(void);
++
++	int (*set_pump)(int enable);
++	int (*set_vbus_supply)(int enable, int srp);
++	int (*set_usbotg_a_mask)(void);
++	int (*set_usbotg_b_mask)(void);
++	int (*is_usbpump_chg)(void);
++
++	int (*is_onkey_assert)(void);
++	int (*is_hookswitch_assert)(void);
++	int (*init)(struct device *dev);
++	int (*deinit)(struct device *dev);
++
++	struct list_head list;	/* callback list */
++	spinlock_t cb_lock;	/* spinlock for callback list */
++};
++
++struct pmic_callback {
++	unsigned long event;	/* the event which callback care about */
++	void (*func)(unsigned long event); /*callback function */
++	struct list_head list;
++};
++
++struct pxa3xx_pmic_regs {
++	unsigned int data:8;
++	unsigned int hit:1;
++	unsigned int mask:1;
++	unsigned int inited:1;
++	unsigned int cacheable:1;
++};
++
++extern void start_calc_time(void);
++extern void end_calc_time(void);
++
++extern int pxa3xx_pmic_write(u8 reg, u8 val);
++extern int pxa3xx_pmic_read(u8 reg, u8 *pval);
++
++extern void pmic_set_ops(struct pmic_ops *ops);
++
++extern int pmic_callback_register(unsigned long event,
++		void (*func)(unsigned long event));
++extern int pmic_callback_unregister(unsigned long event,
++		void (*func)(unsigned long event));
++
++extern int pmic_event_handle(unsigned long event);
++
++extern int pxa3xx_pmic_get_voltage(int cmd, int *pval);
++extern int pxa3xx_pmic_set_voltage(int cmd, int val);
++
++extern int pxa3xx_pmic_check_voltage(int cmd);
++extern int pxa3xx_pmic_enable_voltage(int cmd, int enable);
++extern int pxa3xx_pmic_enable_led(int cmd, int enable);
++/* Check whether USB VBUS is asserted */
++extern int pxa3xx_pmic_is_vbus_assert(void);
++/* Check whether USB VBUS has gone above A-device VBUS valid threshold
++ * Min: 4.4V	Max: N/A
++ */
++extern int pxa3xx_pmic_is_avbusvld(void);
++/* Check whether VBUS has gone above A-device Session Valid threshold
++ * Min: 0.8V	Max: 2.0V
++ */
++extern int pxa3xx_pmic_is_asessvld(void);
++/* Check whether VBUS has gone above B-device Session Valid threshold
++ * Min: 0.8V	Max: 4.0V
++ */
++extern int pxa3xx_pmic_is_bsessvld(void);
++/* Check whether VBUS has gone above B-device Session End threshold
++ * Min: 0.2V	Max: 0.8V
++ */
++extern int pxa3xx_pmic_is_srp_ready(void);
++/* Initialize the USB PUMP */
++extern int pxa3xx_pmic_set_pump(int enable);
++/* Check the events change of PMIC */
++extern int pxa3xx_pmic_event_change(void);
++/* enable/disable VBUS supply */
++extern int pxa3xx_pmic_set_vbus_supply(int enable, int srp);
++/* Set events mask for USB A-device
++ * A-device Sessino Valid event
++ * A-device VBUS Valid event
++ */
++extern int pxa3xx_pmic_set_usbotg_a_mask(void);
++/* Set events mask for USB B-device
++ * B-device Session Valid event
++ * B-device Session end event
++ */
++extern int pxa3xx_pmic_set_usbotg_b_mask(void);
++
++extern int pxa3xx_pmic_is_onkey_assert(void);
++
++extern int pxa3xx_pmic_is_hookswitch_assert(void);
++
++extern int px3xx_pmic_event_enable(unsigned long event, int enable);
++
++
++#endif
++
+diff -ur linux-2.6.32/arch/arm/mach-pxa/include/mach/sgh_msm6k.h kernel/arch/arm/mach-pxa/include/mach/sgh_msm6k.h
+--- linux-2.6.32/arch/arm/mach-pxa/include/mach/sgh_msm6k.h	2009-12-13 12:59:59.879036795 +0200
++++ kernel/arch/arm/mach-pxa/include/mach/sgh_msm6k.h	2009-12-12 16:09:26.466285980 +0200
+@@ -0,0 +1,35 @@
++#ifndef __SGH_MSM6K__
++#define __SGH_MSM6K__
++
++#define CH_RPC	0
++
++enum RPC_PKT_READ_TYPE {
++	RPC_INDICATOR=1,
++	RPC_RESPONSE,
++	RPC_NOTIFICATION,
++};
++
++enum RPC_PKT_WRITE_TYPE {
++	RPC_EXECUTE=1,
++	RPC_GET,
++	RPC_SET,
++	RPC_CFRM,
++};
++
++#define RPC_GSM_CALL_INCOMING			0x0202
++#define RPC_GSM_CALL_STATUS				0x0205
++
++#define RPC_GSM_SEC_PIN_STATUS			0x0501
++#define RPC_GSM_SEC_PHONE_LOCK			0x0502
++#define RPC_GSM_SEC_LOCK_INFOMATION 	0x0508
++
++#define RPC_GSM_SS_INFO					0x0c06
++
++extern void smd_init(void);
++extern int smd_read(int ch, void* buf, int len);
++extern int smd_write(int ch, void *_buf, int len);
++extern void smd_phone_power(int on);
++
++extern void rpc_init(void);
++
++#endif
+diff -ur linux-2.6.32/arch/arm/mach-pxa/include/mach/xscale-pmu.h kernel/arch/arm/mach-pxa/include/mach/xscale-pmu.h
+--- linux-2.6.32/arch/arm/mach-pxa/include/mach/xscale-pmu.h	2009-12-13 13:00:05.321944499 +0200
++++ kernel/arch/arm/mach-pxa/include/mach/xscale-pmu.h	2009-12-12 16:09:26.469613568 +0200
+@@ -0,0 +1,66 @@
++/*
++ *  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.
++ *
++ *
++ * (C) Copyright 2006 Marvell International Ltd.
++ * All Rights Reserved
++ */
++
++#ifndef _XSCALE_PMU_H_
++#define _XSCALE_PMU_H_
++
++#include <linux/types.h>
++
++/*
++ * Different types of events that can be counted by the XScale PMU
++ */
++#define EVT_ICACHE_MISS			0x00
++#define EVT_ICACHE_NO_DELIVER		0x01
++#define EVT_DATA_STALL			0x02
++#define EVT_ITLB_MISS			0x03
++#define EVT_DTLB_MISS			0x04
++#define EVT_BRANCH			0x05
++#define EVT_BRANCH_MISS			0x06
++#define EVT_INSTRUCTION			0x07
++#define EVT_DCACHE_FULL_STALL		0x08
++#define EVT_DCACHE_FULL_STALL_CONTIG	0x09
++#define EVT_DCACHE_ACCESS		0x0A
++#define EVT_DCACHE_MISS			0x0B
++#define EVT_DCACE_WRITE_BACK		0x0C
++#define EVT_PC_CHANGED			0x0D
++#define EVT_BCU_REQUEST			0x10
++#define EVT_BCU_FULL			0x11
++#define EVT_BCU_DRAIN			0x12
++#define EVT_BCU_ECC_NO_ELOG		0x14
++#define EVT_BCU_1_BIT_ERR		0x15
++#define EVT_RMW				0x16
++
++struct pmu_results
++{
++	u32	ccnt_of;
++	u32	ccnt;		/* Clock Counter Register */
++	u32	pmn0_of;
++	u32	pmn0;		/* Performance Counter Register 0 */
++	u32	pmn1_of;
++	u32	pmn1;		/* Performance Counter Register 1 */
++	u32	pmn2_of;
++	u32	pmn2;		/* Performance Counter Register 2 */
++	u32	pmn3_of;
++	u32	pmn3;		/* Performance Counter Register 3 */
++};
++
++#ifdef __KERNEL__
++
++extern struct pmu_results results;
++
++int pmu_claim(void);			/* Claim PMU for usage */
++int pmu_start(u32, u32, u32, u32);	/* Start PMU execution */
++int pmu_stop(struct pmu_results *);	/* Stop perfmon unit */
++int pmu_release(int);			/* Release PMU */
++#endif
++
++#endif /* _XSCALE_PMU_H_ */
++
+diff -ur linux-2.6.32/arch/arm/mach-pxa/pmu.c kernel/arch/arm/mach-pxa/pmu.c
+--- linux-2.6.32/arch/arm/mach-pxa/pmu.c	2009-12-13 13:00:12.875282685 +0200
++++ kernel/arch/arm/mach-pxa/pmu.c	2009-12-12 16:09:26.479614367 +0200
+@@ -0,0 +1,183 @@
++/*
++ * "This software program is available to you under a choice of one of two
++ * licenses.  You may choose to be licensed under either the GNU General Public
++ * License (GPL) Version 2, June 1991, available at
++ * http://www.fsf.org/copyleft/gpl.html, or the BSD License, the text of
++ * which follows:
++ *
++ * Copyright (c) 1996-2005, Intel Corporation. All rights reserved.
++ *
++ * Redistribution and use in source and binary forms, with or without
++ * modification, are permitted provided that the following conditions are met:
++ *
++ * Redistributions of source code must retain the above copyright notice, this
++ * list of conditions and the following disclaimer.
++ *
++ * Redistributions in binary form must reproduce the above copyright notice, this
++ * list of conditions and the following disclaimer in the documentation and/or
++ * other materials provided with the distribution.
++ *
++ * Neither the name of the Intel Corporation ("Intel") nor the names of its
++ * contributors may be used to endorse or promote products derived from this
++ * software without specific prior written permission.
++ *
++ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
++ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
++ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
++ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
++ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
++ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
++ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
++ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
++ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
++ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE."
++ */
++
++/*
++ *  FILENAME: pmu.c
++ *
++ *  CORE STEPPING:
++ *
++ *  PURPOSE: contains all PMU C function.
++ *
++ *  (C) Copyright 2006 Marvell International Ltd.
++ *  All Rights Reserved
++ */
++
++#include <linux/kernel.h>
++#include <linux/module.h>
++#include <linux/init.h>
++#include <linux/platform_device.h>
++#include <mach/pmu.h>
++#include <asm/types.h>
++#include <mach/pxa3xx-regs.h>
++#include <mach/hardware.h>
++#include <asm/io.h>
++
++static struct pxa3xx_pmu_info *pmu_info;
++
++/*
++ * Select one event including PMU and PML envent for PMU counter
++ *
++ * @par
++ *	This function selects one event including Manzano and Monahans event.
++ *	When type is Monahans PML Event, it is Monahans PML Event Number OR
++ *	PXA3xx_EVENT_MASK. Other words, when type is Manzano event, bit31 is
++ *	zero. When type is Monahans PML Event, bit31 is one.
++ * @par
++ *	We only use Monahans PML first four event selectors because manzano
++ *	has only 4 counters and every selector can choose all PML events.
++ *	We use 1:1 map from PMU counter to PML selector. So counter 0 use
++ *	PML_SEL0, counter1 use PML_SEL1 and so on.
++ * @param
++ *	counter  PMU Counter Number. It must be between 0 and 3
++ *	type     PMU And PML type
++ * @return
++ *	old event type before call this function.
++ * @remarks
++ *	required kernel/supervisor mode
++ */
++int pmu_select_event(int counter, int type)
++{
++	u32 oldevent, value, pmuevent, shift;
++
++	if(counter < 0 || counter > 3) {
++		return PMU_EVENT_INVALIDATE;
++	}
++	shift = counter * 8;
++
++	value = pmu_read_reg((u32)PMU_EVTSEL);
++	pmuevent = (value >> shift) & 0xFF;
++
++	if (pmuevent >= PMU_EVENT_ASSP_0 && pmuevent <= PMU_EVENT_ASSP_3) {
++		oldevent = PXA3xx_EVENT_MASK |
++				(*(pmu_info->pmu_base + (counter << 2)));
++	} else {
++		oldevent = pmuevent;
++	}
++
++	if(type & PXA3xx_EVENT_MASK) {
++		/* PML Event */
++		value &= ~(0xFF << shift);
++		value |= (PMU_EVENT_ASSP_0 + counter) << shift;
++		*(pmu_info->pmu_base + (counter << 2)) =
++			type & (~PXA3xx_EVENT_MASK);
++	} else {
++		/* PMU Event */
++		value &= ~(0xFF << shift);
++		value |= (type & 0xFF) << shift;
++	}
++	pmu_write_reg((u32)PMU_EVTSEL, value);
++
++	return oldevent;
++}
++
++#ifdef CONFIG_PM
++static int pxa3xx_pmu_suspend(struct platform_device *pdev, pm_message_t state)
++{
++	return 0;
++}
++
++static int pxa3xx_pmu_resume(struct platform_device *pdev)
++{
++	return 0;
++}
++#else
++#define pxa3xx_pmu_suspend    NULL
++#define pxa3xx_pmu_resume     NULL
++#endif
++
++static int __init pxa3xx_pmu_probe(struct platform_device *pdev)
++{
++	struct resource *res;
++
++	pmu_info = kzalloc(sizeof(struct pxa3xx_pmu_info), GFP_KERNEL);
++
++	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "pmu_regs");
++	if (!res) goto err;
++	pmu_info->pmu_base = ioremap(res->start, res->end - res->start + 1);
++	return 0;
++err:
++	printk("pxa3xx PMU init failed\n");
++	return -EIO;
++}
++
++static int pxa3xx_pmu_remove(struct platform_device *pdev)
++{
++	struct resource *res;
++
++	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "pmu_regs");
++	if (!res) goto err;
++	iounmap(pmu_info->pmu_base);
++	kfree(pmu_info);
++	return 0;
++err:
++	printk("pxa3xx PMU remove failed\n");
++	return -EIO;
++}
++
++static struct platform_driver pxa3xx_pmu_driver = {
++	.driver = {
++		.name	= "pxa3xx-pmu",
++	},
++	.probe		= pxa3xx_pmu_probe,
++	.remove		= pxa3xx_pmu_remove,
++#ifdef CONFIG_PM
++	.suspend	= pxa3xx_pmu_suspend,
++	.resume		= pxa3xx_pmu_resume,
++#endif
++};
++
++static int __init pxa3xx_pmu_init(void)
++{
++	return platform_driver_register(&pxa3xx_pmu_driver);
++}
++
++static void __exit pxa3xx_pmu_exit(void)
++{
++	platform_driver_unregister(&pxa3xx_pmu_driver);
++}
++
++module_init(pxa3xx_pmu_init);
++module_exit(pxa3xx_pmu_exit);
++
+diff -ur linux-2.6.32/arch/arm/mach-pxa/pmu_ll.S kernel/arch/arm/mach-pxa/pmu_ll.S
+--- linux-2.6.32/arch/arm/mach-pxa/pmu_ll.S	2009-12-13 13:00:17.648612716 +0200
++++ kernel/arch/arm/mach-pxa/pmu_ll.S	2009-12-12 16:09:26.479614367 +0200
+@@ -0,0 +1,204 @@
++@ "This software program is available to you under a choice of one of two
++@ licenses.  You may choose to be licensed under either the GNU General Public
++@ License (GPL) Version 2, June 1991, available at
++@ http://www.fsf.org/copyleft/gpl.html, or the BSD License, the text of
++@ which follows:
++@
++@ Copyright (c) 1996-2005, Intel Corporation. All rights reserved.
++@
++@ Redistribution and use in source and binary forms, with or without
++@ modification, are permitted provided that the following conditions are met:
++@
++@ Redistributions of source code must retain the above copyright notice, this
++@ list of conditions and the following disclaimer.
++@
++@ Redistributions in binary form must reproduce the above copyright notice, this
++@ list of conditions and the following disclaimer in the documentation and/or
++@ other materials provided with the distribution.
++@
++@ Neither the name of the Intel Corporation ("Intel") nor the names of its
++@ contributors may be used to endorse or promote products derived from this
++@ software without specific prior written permission.
++@
++@ THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
++@ AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
++@ IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
++@ DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
++@ FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
++@ DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
++@ SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
++@ CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
++@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
++@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE."
++@
++@  FILENAME:       pmu_ll.S
++@
++@  PURPOSE:        Provides low level PMU primitive functions written specifically for
++@                  the Bulverde/Mainstone processor/platform.  Specially design to fit
++@                  into Intel VTUNE Architecture
++@
++@
++@  LAST MODIFIED:  10/31/02
++@******************************************************************************
++@
++@
++@ List of primitive functions in this source code include:
++@
++	.global pmu_read_reg
++	.global pmu_write_reg
++
++	.text
++
++@
++@ pmu_read_reg - Read the PMU Register
++@
++@ Description:
++@	This routine reads the designated PMU register via CoProcesser 14.
++@
++@ Input Parameters:
++@	r0 - arg1, PMU register number to read.  Number between 0 to 8
++@	if r0 contains:
++@		0 -> PMNC,	PMU Control Register
++@		1 -> CCNT,	PMU Clock Counter
++@		2 -> PMN0,	PMU Count Register 0
++@		3 -> PMN1,	PMU Count Register 1
++@		4 -> PMN2,	PMU Count Register 2
++@		5 -> PMN3,	PMU Count Register 3
++@		6 -> INTEN,	PMU Interupt Enable Register
++@		7 -> FLAG,	PMU Overflow Flag Status Register
++@		8 -> EVTSEL	PMU Event Select Register
++@
++@ Returns:
++@	r0 - 32-bit value read from CoProcessor
++@
++@ Registers Modified:
++@	CoProcessor Register Modified: None
++@	General Purpose Registers Modified: r0
++@
++@ NOTE:
++@	Currently not support THUMB mode
++@	Error checking not included
++
++pmu_read_reg:
++
++	cmp	r0, #8
++	addls	pc, pc, r0, lsl #2
++	b	RRet
++	b	RdPMNC
++	b	RdCCNT
++	b	RdPMN0
++	b	RdPMN1
++	b	RdPMN2
++	b	RdPMN3
++	b	RdINTEN
++	b	RdFLAG
++	b	RdEVTSEL
++
++RdPMNC:
++	mrc	p14, 0, r0, c0, c1, 0	@ Read PMNC
++	b	RRet
++RdCCNT:
++	mrc	p14, 0, r0, c1, c1, 0	@ Read CCNT
++	b	RRet
++RdPMN0:
++	mrc	p14, 0, r0, c0, c2, 0	@ Read PMN0
++	b	RRet
++RdPMN1:
++	mrc	p14, 0, r0, c1, c2, 0	@ Read PMN1
++	b	RRet
++RdPMN2:
++	mrc	p14, 0, r0, c2, c2, 0	@ Read PMN2
++	b	RRet
++RdPMN3:
++	mrc	p14, 0, r0, c3, c2, 0	@ Read PMN3
++	b	RRet
++RdINTEN:
++	mrc	p14, 0, r0, c4, c1, 0	@ Read INTEN
++	b	RRet
++RdFLAG:
++	mrc	p14, 0, r0, c5, c1, 0	@ Read FLAG
++	b	RRet
++RdEVTSEL:
++	mrc	p14, 0, r0, c8, c1, 0	@ Read EVTSEL
++
++RRet:
++	mov	pc, lr			@ return
++
++
++@
++@ pmu_write_reg - Writes to the PMU Register
++@
++@ Description:
++@	This routine writes to the designated PMU register via CoProcesser 14.
++@
++@ Input Parameters:
++@	r0 - arg1 - PMU register number to write
++@	r1 - arg2 - Value to write to PMU register
++@
++@	if r0 contains:
++@		0 -> PMNC,	PMU Control Register
++@		1 -> CCNT,	PMU Clock Counter
++@		2 -> PMN0,	PMU Count Register 0
++@		3 -> PMN1,	PMU Count Register 1
++@		4 -> PMN2,	PMU Count Register 2
++@		5 -> PMN3,	PMU Count Register 3
++@		6 -> INTEN,	PMU Interupt Enable Register
++@		7 -> FLAG,	PMU Overflow Flag Status Register
++@		8 -> EVTSEL	PMU Event Select Register
++@
++@ Returns:
++@	None
++@
++@ Registers Modified:
++@	CoProcessor Register Modified: PMU Register
++@	General Purpose Registers Modified: None
++@
++@NOTE:
++@	Currently not support THUMB mode
++@	Error checking not included
++
++pmu_write_reg:
++
++	cmp	r0, #8
++	addls	pc, pc, r0, lsl #2
++	b	WRet
++	b	WrPMNC
++	b	WrCCNT
++	b	WrPMN0
++	b	WrPMN1
++	b	WrPMN2
++	b	WrPMN3
++	b	WrINTEN
++	b	WrFLAG
++	b	WrEVTSEL
++
++WrPMNC:
++	mcr	p14, 0, r1, c0, c1, 0	@ Write PMNC
++	b	WRet
++WrCCNT:
++	mcr	p14, 0, r1, c1, c1, 0	@ Write CCNT
++	b	WRet
++WrPMN0:
++	mcr	p14, 0, r1, c0, c2, 0	@ Write PMN0
++	b	WRet
++WrPMN1:
++	mcr	p14, 0, r1, c1, c2, 0	@ Write PMN1
++	b	WRet
++WrPMN2:
++	mcr	p14, 0, r1, c2, c2, 0	@ Write PMN2
++	b	WRet
++WrPMN3:
++	mcr	p14, 0, r1, c3, c2, 0	@ Write PMN3
++	b	WRet
++WrINTEN:
++	mcr	p14, 0, r1, c4, c1, 0	@ Write INTEN
++	b	WRet
++WrFLAG:
++	mcr	p14, 0, r1, c5, c1, 0	@ Write FLAG
++	b	WRet
++WrEVTSEL:
++	mcr	p14, 0, r1, c8, c1, 0	@ Write EVTSEL
++
++WRet:
++	mov	pc, lr			@ return
++
+diff -ur linux-2.6.32/arch/arm/mach-pxa/prm.c kernel/arch/arm/mach-pxa/prm.c
+--- linux-2.6.32/arch/arm/mach-pxa/prm.c	2009-12-13 13:00:22.645696759 +0200
++++ kernel/arch/arm/mach-pxa/prm.c	2009-12-12 16:09:26.479614367 +0200
+@@ -0,0 +1,1266 @@
++/*
++ * Monahans Profiler Resource Manager
++ *
++ * Copyright (C) 2004, Intel Corporation(chao.xie@intel.com).
++ *
++ * This software program is licensed subject to the GNU General Public License
++ * (GPL).Version 2,June 1991, available at http://www.fsf.org/copyleft/gpl.html
++ *
++ *(C) Copyright 2006 Marvell International Ltd.
++ * All Rights Reserved
++ */
++#include <linux/module.h>
++#include <linux/types.h>
++#include <linux/init.h>
++#include <linux/errno.h>
++#include <linux/sched.h>
++#include <asm/current.h>
++#include <linux/proc_fs.h>
++#include <linux/rwsem.h>
++#include <linux/interrupt.h>
++#include <mach/prm.h>
++#include <mach/pmu.h>
++#include <mach/dvfm.h>
++#include <mach/pxa3xx_dvfm.h>
++
++/*#define DEBUG
++ */
++
++#ifdef DEBUG
++#define DPRINTK(fmt,args...)	\
++	do { printk(KERN_DEBUG "%s: " fmt, __FUNCTION__ , ## args); } while (0)
++#else
++#define DPRINTK(fmt,args...)	do {} while (0)
++#endif
++
++#define ASSERT_PRIORITY(pri) \
++	if ((pri) < PRI_LOWEST || (pri) > PRI_HIGHEST) \
++		return -EINVAL;
++#define ASSERT_CLIENT_ID(client) \
++	if ((client) < 0 || (client) > MAX_CLIENTS) \
++		return -EINVAL;
++#define ASSERT_RESOURCE_ID(resource) \
++	if ((resource) < 0 || (resource) > RESOURCE_NUM) \
++		return -EINVAL;
++#define ASSERT_GROUP_ID(group) \
++	if ((group) < 0 || (group) > MAX_GROUPS) \
++		return -EINVAL;
++
++#define IS_PRM_RESOURCE(reg)	((reg) >= PMU_CCNT && (reg) <= PMU_PMN3)
++#define PMU_PRM(reg)		(reg - 1)
++
++#define IS_HIGHER_PRIORITY(h1, h2)	((h1) > (h2))
++#define for_each_lower_priority(index, pri)	\
++	for (index = pri - 1; index >= PRI_LOWEST; index = index - 1)
++
++#define STATE_UNDEF		0x1
++#define STATE_ACTIVE		0x2
++#define STATE_APPROPRIATED	0x3
++
++static struct prm_resource prm_resources[RESOURCE_NUM];
++static struct prm_client *prm_clients[MAX_CLIENTS];
++static struct prm_client *prm_pmu_client;
++
++struct rw_semaphore prm_sem;
++
++#ifdef DEBUG
++static struct proc_dir_entry *clients_root;
++static struct proc_dir_entry *resources_root;
++static struct proc_dir_entry *prm_root;
++
++#define proc_dump_end(len, page, start, off, count, eof) \
++do {						\
++	if (len <= off + count) *eof = 1;	\
++	*start = page + off;			\
++	len -= off;				\
++	if (len > count) len = count;		\
++	if (len < 0) len = 0;			\
++} while (0)
++
++static int dump_group(char *page, char **start, off_t off,
++		int count, int *eof, void *data)
++{
++	char *buf = page;
++	int len;
++	struct prm_group *group = (struct prm_group *)data;
++
++	buf += sprintf(buf, "address: 0x%x\n member_cnt: %u\n"
++		" appropriated_cnt: %u\n",
++		(unsigned int)group, group->member_cnt,
++		group->appropriated_cnt);
++	len = buf - page;
++	proc_dump_end(len, page, start, off, count, eof);
++	return len;
++}
++
++static int dump_resource_state(char *page, char **start, off_t off,
++		int count, int *eof, void *data)
++{
++	int client_id = -1, group_id = -1;
++	int i, len;
++	char *buf = page;
++	struct prm_resource_state *state = (struct prm_resource_state *)data;
++
++	if (state->allocate) {
++		for (i = 0; i < MAX_CLIENTS; i++) {
++			if (prm_clients[i] && prm_clients[i] == state->allocate) {
++				client_id = i;
++				break;
++			}
++		}
++		for (i = 0; i < MAX_GROUPS; i++) {
++			if (state->allocate->groups[i] &&
++			    state->allocate->groups[i] == state->group) {
++				group_id = i;
++				break;
++			}
++		}
++	}
++	buf += sprintf(buf, "allocate: 0x%x(%d)\n group: 0x%x(%d)\n"
++		" active: %u\n resource: 0x%x\n",
++		(unsigned int)state->allocate, client_id,
++		(unsigned int)state->group,
++		group_id, state->active, (unsigned int)state->resource);
++	len = buf - page;
++	proc_dump_end(len, page, start, off, count, eof);
++	return len;
++}
++
++static int dump_client(char *page, char **start, off_t off,
++		int count, int *eof, void *data)
++{
++	int i, len;
++	char *buf = page;
++	struct prm_client *client = (struct prm_client *)data;
++
++	buf += sprintf(buf, "address: 0x%x\n id: %u\n pid: %u\n"
++		" priority: %u\n name: %s\n group_cnt: %u\n",
++		(unsigned int)client, client->id, client->pid,
++		client->priority, client->name, client->group_cnt);
++	for (i = 0 ;i < MAX_GROUPS; i++) {
++		if (client->groups[i])
++			buf += sprintf(buf, "group%u address: 0x%x\n",
++				i, (unsigned int)client->groups[i]);
++	}
++	len = buf - page;
++	proc_dump_end(len, page, start, off, count, eof);
++	return len;
++}
++
++static int dump_resource(char *page, char **start, off_t off,
++		int count, int *eof, void *data)
++{
++	int i, client_id = -1, len;
++	char *buf = page;
++	struct prm_resource *resource = (struct prm_resource *)data;
++
++	for (i = 0; i < MAX_CLIENTS; i++) {
++		if (prm_clients[i] && prm_clients[i] == resource->access) {
++			client_id = i;
++			break;
++		}
++	}
++	buf += sprintf(buf, " address:0x%x\n access: 0x%x(%d)\n id: %u\n",
++		(unsigned int)resource, (unsigned int)resource->access,
++		client_id, resource->id);
++	len = buf - page;
++	proc_dump_end(len, page, start, off, count, eof);
++	return len;
++}
++
++static void proc_add_client(struct prm_client *client)
++{
++	char buf[16];
++
++	sprintf(buf, "client%d", client->id);
++	client->dir = proc_mkdir(buf, clients_root);
++	create_proc_read_entry("info", 0, client->dir, dump_client, client);
++}
++
++static void proc_del_client(struct prm_client *client)
++{
++	char buf[16];
++
++	remove_proc_entry("info", client->dir);
++	sprintf(buf, "client%d", client->id);
++	remove_proc_entry(buf, clients_root);
++}
++
++static void proc_add_group(struct prm_client *client,
++		struct prm_group *group, unsigned int group_id)
++{
++	char buf[16];
++
++	sprintf(buf, "group%d", group_id);
++	group->dir = proc_mkdir(buf, client->dir);
++	create_proc_read_entry("info", 0, group->dir, dump_group, group);
++}
++
++static void proc_del_group(struct prm_client*client,
++		struct prm_group *group, unsigned int group_id)
++{
++	char buf[32];
++
++	remove_proc_entry("info", group->dir);
++	sprintf(buf, "group%d", group_id);
++	remove_proc_entry(buf, client->dir);
++}
++
++static void proc_add_resource(struct prm_resource *resource)
++{
++	char buf[16];
++
++	sprintf(buf, "resource%d", resource->id);
++	resource->dir = proc_mkdir(buf, resources_root);
++	create_proc_read_entry("info", 0, resource->dir,
++		dump_resource, resource);
++}
++
++static void proc_del_resource(struct prm_resource *resource)
++{
++	char buf[16];
++
++	remove_proc_entry("info", resource->dir);
++	sprintf(buf, "resource%d", resource->id);
++	remove_proc_entry(buf, resources_root);
++}
++
++static void proc_add_resource_state(struct prm_resource_state *state,
++		unsigned int priority)
++{
++	char buf[16];
++
++	sprintf(buf, "state%d", priority);
++	state->dir = proc_mkdir(buf, state->resource->dir);
++	create_proc_read_entry("info", 0, state->dir,
++		dump_resource_state, state);
++}
++
++static void proc_del_resource_state(struct prm_resource_state *state,
++		unsigned int priority)
++{
++	char buf[16];
++
++	remove_proc_entry("info", state->dir);
++	sprintf(buf, "state%d", priority);
++	remove_proc_entry(buf, state->resource->dir);
++}
++
++static void proc_commit_resource(struct prm_resource *resource)
++{
++	char buf[32];
++
++	remove_proc_entry("access", resource->dir);
++	sprintf(buf, "/proc/prm/clients/%s", resource->access->dir->name);
++	proc_symlink("access", resource->dir, buf);
++}
++
++static void proc_allocate_resource(struct prm_resource_state *state)
++{
++	char buf[32], path[64];
++
++	remove_proc_entry("allocate", state->dir);
++
++	sprintf(path, "/proc/prm/clients/%s/%s",
++		state->allocate->dir->name, state->group->dir->name);
++	proc_symlink("group", state->dir, path);
++	sprintf(buf, "resource%d_state%d",
++		state->resource->id, state->allocate->priority);
++	sprintf(path, "/proc/prm/resources/%s/%s",
++		state->resource->dir->name, state->dir->name);
++	proc_symlink(buf, state->group->dir, path);
++}
++
++static void proc_free_resource(struct prm_resource_state *state)
++{
++	char buf[32];
++
++	sprintf(buf, "resource%d_state%d",
++		state->resource->id, state->allocate->priority);
++	remove_proc_entry("access", state->resource->dir);
++	remove_proc_entry("allocate", state->dir);
++	remove_proc_entry("group", state->dir);
++	remove_proc_entry(buf, state->group->dir);
++}
++
++static void proc_prm_init(void)
++{
++	prm_root = proc_mkdir("prm", NULL);
++	clients_root = proc_mkdir("clients", prm_root);
++	resources_root = proc_mkdir("resources", prm_root);
++}
++
++static void proc_prm_exit(void)
++{
++	remove_proc_entry("clients", prm_root);
++	remove_proc_entry("resources", prm_root);
++	remove_proc_entry("prm", NULL);
++}
++#else
++static void proc_add_client(struct prm_client *client) {}
++static void proc_del_client(struct prm_client *client) {}
++static void proc_add_group(struct prm_client *client,
++	struct prm_group *group, unsigned int group_id) {}
++static void proc_del_group(struct prm_client*client,
++	struct prm_group *group, unsigned int group_id) {}
++static void proc_add_resource(struct prm_resource *resource) {}
++static void proc_del_resource(struct prm_resource *resource) {}
++static void proc_add_resource_state(struct prm_resource_state *state,
++	unsigned int priority) {}
++static void proc_del_resource_state(struct prm_resource_state *state,
++	unsigned int priority) {}
++static void proc_commit_resource(struct prm_resource *resource) {}
++static void proc_allocate_resource(struct prm_resource_state *state) {}
++static void proc_free_resource(struct prm_resource_state *state) {}
++static void proc_prm_init(void) {}
++static void proc_prm_exit(void) {}
++#endif
++
++/*****************************************************************************/
++/*                                                                           */
++/*                  Profiler Resource Manager                                */
++/*                                                                           */
++/*****************************************************************************/
++
++static void clear_state(struct prm_resource_state *state)
++{
++	state->allocate = NULL;
++	state->group = NULL;
++	state->active = STATE_UNDEF;
++	/* the state has been deleted from the group resource list */
++	INIT_LIST_HEAD(&(state->entry));
++}
++
++static int group_commited(struct prm_client *client,
++		struct prm_group *group)
++{
++	struct prm_resource_state *state;
++	struct prm_resource *resource;
++	struct list_head *pos;
++
++	list_for_each(pos, &(group->resources)) {
++		state = list_entry(pos, struct prm_resource_state, entry);
++		resource = state->resource;
++		if (resource->access != client) {
++			return 0;
++		}
++	}
++	return 1;
++}
++
++static int try_to_access_group(struct prm_client *client,
++		struct prm_group *group, int set_state)
++{
++	struct prm_resource_state *state;
++	struct prm_resource *resource;
++	int ret = 0;
++	struct list_head *pos;
++
++	DPRINTK("client <%d> try to access group <%d>, set_state as <%d>\n",
++		(unsigned int)client->id, (unsigned int)group->id, set_state);
++	list_for_each(pos, &(group->resources)) {
++		state = list_entry(pos, struct prm_resource_state, entry);
++		resource = state->resource;
++		if (resource->access != NULL && resource->access != client &&
++		    IS_HIGHER_PRIORITY(resource->access->priority,
++		    client->priority)) {
++			if (set_state) {
++				state->active = STATE_APPROPRIATED;
++				group->appropriated_cnt++;
++			}
++			ret++;
++		}
++	}
++	DPRINTK("try_to_access() return :%d\n", ret);
++	return ret;
++}
++
++static struct prm_client * get_resource_access(struct prm_resource *resource)
++{
++	if (resource)
++		return resource->access;
++	else	/*for access the isr and control regs of PMU */
++		return (struct prm_client *)(
++			(unsigned long)prm_resources[PRM_CCNT].access &
++			(unsigned long)prm_resources[PRM_PMN0].access &
++			(unsigned long)prm_resources[PRM_PMN1].access &
++			(unsigned long)prm_resources[PRM_PMN2].access &
++			(unsigned long)prm_resources[PRM_PMN3].access
++		);
++}
++
++static void unload_isr(struct prm_client *client)
++{
++	if (prm_pmu_client == client || get_resource_access(NULL) == client)
++		prm_pmu_client = NULL;
++}
++
++static void load_isr(struct prm_client *client)
++{
++	if (prm_pmu_client != client && get_resource_access(NULL) == client)
++		prm_pmu_client = client;
++}
++
++/* this function will be invoked with locked */
++static int set_resource_access(struct prm_resource *resource,
++		struct prm_client *client)
++{
++	struct prm_resource_state *state, *owner_state;
++	struct prm_group *group, *owner_group;
++	struct prm_client *owner;
++	int ret = 0;
++
++	if (client == NULL) {
++	/* The client will free the committed resources to the appropriated
++	 * lower client. And notification will be sent so as to give the lower
++	 * priority client a chance to commit resources if:
++	 * 1. all the resources of the lower priority group that resource
++	 *    belongs to are committable
++	 * 2. lower priority client hasn't committed above group resources
++	 * Note: the notified client is unnessarily the appropriated client.
++	 */
++		int index;
++
++		DPRINTK("client <%d> give up resource <%d>\n",
++		(unsigned int)resource->access->id, (unsigned int)resource->id);
++		unload_isr(resource->access);
++		owner = resource->access;
++		resource->access = NULL;
++		resource->priority[owner->priority].active = STATE_UNDEF;
++		for_each_lower_priority(index, owner->priority) {
++			state = &(resource->priority[index]);
++			DPRINTK("   its state of lower priority <%d> is <%d>\n",
++				index, state->active);
++			if (state->active == STATE_APPROPRIATED) {
++				DPRINTK("client <%d> return resource <%d>"
++					" to client <%d>\n", owner->id,
++					resource->id, state->allocate->id);
++				group = state->group;
++				group->appropriated_cnt--;
++				DPRINTK("resource group <%d> of client <%d>"
++					" has <%d> resources appropriated\n",
++					group->id, state->allocate->id,
++					group->appropriated_cnt);
++			}
++			if (state->group &&
++			    state->group->appropriated_cnt == 0 &&
++			    state->allocate &&
++			    !group_commited(state->allocate, state->group)) {
++				ret = try_to_access_group(state->allocate,
++					state->group, 1);
++				if (ret < 0)
++					return ret;
++				else if (ret == 0) {
++					/* state->active = STATE_UNDEF;
++					 */
++					/* ISR will not reload, because now the
++					 * group has not be commited again.
++					 * The isr should be loaded when commit
++					 */
++					if (state->allocate->notify) {
++						up_write(&prm_sem);
++						DPRINTK("client <%d> notified"
++						    " with PRM_RES_READY\n",
++						(unsigned int)state->allocate->id);
++						state->allocate->notify(PRM_RES_READY,
++							state->group->id,
++							state->allocate->client_data);
++						down_write(&prm_sem);
++					}
++					break;
++				}
++			}
++		}
++	}
++	else {
++		struct prm_resource *group_resource;
++		struct list_head *pos;
++
++		owner = resource->access;
++
++		if (owner == client){
++			DPRINTK("client <%d> commits resource <%d>:"
++				" already commited\n",
++				(unsigned int)client->id,
++				(unsigned int)resource->id);
++			return 0;
++		}
++		if (!owner)
++			unload_isr(owner);
++		resource->access = client;
++		resource->priority[client->priority].active = STATE_ACTIVE;
++		load_isr(client);
++		if (owner == NULL) {
++			DPRINTK("client <%d> commits resource <%d>:"
++				" from free list\n",
++		                (unsigned int)client->id,
++				(unsigned int)resource->id);
++				return 0;
++		} else {
++			DPRINTK("client <%d> commits resource <%d>:"
++				" from client <%d>\n\n",
++				(unsigned int)client->id,
++				(unsigned int)resource->id, owner->id);
++		}
++
++		owner_state = &(resource->priority[owner->priority]);
++		owner_state->active = STATE_APPROPRIATED;
++		owner_group = owner_state->group;
++		if (owner_group->appropriated_cnt++ == 0) {
++			list_for_each(pos, &(owner_group->resources)) {
++				state = list_entry(pos,
++					struct prm_resource_state, entry);
++				group_resource = state->resource;
++				if (group_resource->access == owner)
++					ret = set_resource_access(
++						group_resource, NULL);
++				if (ret)
++					return ret;
++			}
++			if (owner->notify) {
++				up_write(&prm_sem);
++				DPRINTK("client <%d> notified with"
++					" PRM_RES_APPROPRIATED\n",
++					(unsigned int)owner->id);
++				owner->notify(PRM_RES_APPROPRIATED,
++				owner_group->id, owner->client_data);
++				down_write(&prm_sem);
++			}
++		}
++	}
++	return 0;
++}
++
++int prm_open_session(prm_priority priority, char *name,
++		clientcallback callback, void *data)
++{
++	struct prm_client * client;
++	unsigned int name_len;
++	int i = 0;
++
++	ASSERT_PRIORITY(priority);
++	if (!name) {
++		return -EINVAL;
++	}
++	/* protect for read */
++	down_read(&prm_sem);
++	for (i = 0;i < MAX_CLIENTS;i++) {
++		if (prm_clients[i] == NULL)
++			break;
++	}
++	up_read(&prm_sem);
++
++	if (i == MAX_CLIENTS)
++		return -ENOENT;
++
++	name_len = strlen(name);
++	client = (struct prm_client *)
++		kmalloc(sizeof(struct prm_client) + name_len + 1, GFP_KERNEL);
++	if (!client)
++		return -ENOMEM;
++	memset(client, 0x0, sizeof(struct prm_client));
++	client->id = i;
++	client->pid = current->pid;
++	client->priority = priority;
++	client->notify = callback;
++	client->client_data = data;
++	client->name = (char *)(client + 1);
++	strncpy(client->name, name, name_len);
++	client->name[name_len] = '\0';
++
++	for(i = 0;i < MAX_GROUPS;i++)
++		client->groups[i] = NULL;
++
++	down_write(&prm_sem);
++	if (prm_clients[client->id] != NULL) {
++		up_write(&prm_sem);
++		kfree(client);
++		return -ENOENT;
++	}
++	prm_clients[client->id] = client;
++	up_write(&prm_sem);
++	proc_add_client(client);
++
++	DPRINTK("client<%d>(%s) open a session with priority <%d>\n",
++		client->id, name, priority);
++
++	return client->id;
++}
++
++int prm_close_session(unsigned int client_id)
++{
++	struct prm_client *client;
++
++	ASSERT_CLIENT_ID(client_id);
++
++	down_write(&prm_sem);
++	client = prm_clients[client_id];
++	/* resources should be freed before close seesion */
++	if (client->group_cnt) {
++		up_write(&prm_sem);
++		return -EPERM;
++	}
++	prm_clients[client_id] = NULL;
++	up_write(&prm_sem);
++	proc_del_client(client);
++	kfree(client);
++
++	DPRINTK("client<%d> closed its session\n", client_id);
++
++	return 0;
++}
++
++/* allocate resource, but can not access it now */
++int prm_allocate_resource(unsigned int client_id,
++		prm_resource_id res_id, unsigned int group_id)
++{
++	struct prm_client *client;
++	struct prm_resource *resource;
++	struct prm_resource_state *state;
++	struct prm_group *group;
++
++	ASSERT_CLIENT_ID(client_id);
++	ASSERT_RESOURCE_ID(res_id);
++	ASSERT_GROUP_ID(group_id);
++
++	DPRINTK("allocate resource for client <%d> with resource <%d>"
++		" for group <%d>\n", client_id, res_id, group_id);
++	down_write(&prm_sem);
++	client = prm_clients[client_id];
++	if (!client) {
++		up_write(&prm_sem);
++		return -EINVAL;
++	}
++	resource = &(prm_resources[res_id]);
++	state = &(resource->priority[client->priority]);
++
++	/* The resource in the client->priority has been reserved */
++	if (state->allocate) {
++		up_write(&prm_sem);
++		return -EPERM;
++	}
++	else
++		state->allocate = client;
++	group = client->groups[group_id];
++	up_write(&prm_sem);
++
++	if (group == NULL) {
++		group = (struct prm_group *)
++			kmalloc(sizeof(struct prm_group), GFP_KERNEL);
++		if (group == NULL)
++			return -ENOMEM;
++
++		INIT_LIST_HEAD(&(group->resources));
++		group->id = group_id;
++		group->appropriated_cnt = 0;
++		group->member_cnt = 1;
++		proc_add_group(client, group, group_id);
++
++		down_write(&prm_sem);
++		if (client->groups[group_id]) {
++			up_write(&prm_sem);
++			kfree(group);
++			down_write(&prm_sem);
++			group = client->groups[group_id];
++		}
++		else {
++			client->groups[group_id] = group;
++			client->group_cnt++;
++		}
++	}
++	else {
++		down_write(&prm_sem);
++		client->groups[group_id]->member_cnt++;
++	}
++	list_add(&(state->entry), &(group->resources));
++	state->group = group;
++	state->active = STATE_UNDEF;
++	up_write(&prm_sem);
++	proc_allocate_resource(state);
++
++	return 0;
++}
++
++int prm_free_resources(unsigned int client_id, unsigned int group_id)
++{
++	struct prm_client *client;
++	struct prm_resource *resource;
++	struct prm_group *group;
++	struct prm_resource_state *state;
++	int ret = -EINVAL;
++	struct list_head *pos, *n;
++
++	ASSERT_CLIENT_ID(client_id);
++	ASSERT_GROUP_ID(group_id);
++
++	down_write(&prm_sem);
++	client = prm_clients[client_id];
++	if (!client) {
++		up_write(&prm_sem);
++		return -EINVAL;
++	}
++	group = client->groups[group_id];
++	if (!group) {
++		up_write(&prm_sem);
++		return -EINVAL;
++	}
++
++	list_for_each_safe(pos, n, &(group->resources)) {
++		state = list_entry(pos, struct prm_resource_state, entry);
++		resource = state->resource;
++		if (get_resource_access(resource) == client) {
++			ret = set_resource_access(resource, NULL);
++			if (ret) {
++				up_write(&prm_sem);
++				return ret;
++			}
++		}
++#if 0
++		else if (state->active == STATE_APPROPRIATED)
++			group->appropriated_cnt--;
++#endif
++		proc_free_resource(state);
++		list_del(pos);
++		clear_state(state);
++	}
++	client->group_cnt--;
++	client->groups[group_id] = NULL;
++	up_write(&prm_sem);
++	proc_del_group(client, group, group_id);
++	kfree(group);
++
++	return 0;
++}
++
++int prm_commit_resources(unsigned int client_id, unsigned int group_id)
++{
++	struct prm_client *client;
++	struct prm_group *group;
++	struct prm_resource_state *state;
++	struct prm_resource *resource;
++	struct list_head *pos;
++	int ret;
++
++	ASSERT_CLIENT_ID(client_id);
++	ASSERT_GROUP_ID(group_id);
++
++	DPRINTK("client <%d> commit resource group <%d>\n",
++		client_id, group_id);
++	down_write(&prm_sem);
++	client = prm_clients[client_id];
++	if (!client) {
++		up_write(&prm_sem);
++		return -EINVAL;
++	}
++	group = client->groups[group_id];
++	if (!group) {
++		up_write(&prm_sem);
++		return -EINVAL;
++	}
++
++	ret = try_to_access_group(client, group, 0);
++	if (ret) {
++		up_write(&prm_sem);
++		return ret;
++	}
++
++	list_for_each(pos, &(group->resources)) {
++		state = list_entry(pos, struct prm_resource_state, entry);
++		resource = state->resource;
++		ret = set_resource_access(resource, client);
++		if (ret) {
++			up_write(&prm_sem);
++			return ret;
++		}
++		proc_commit_resource(resource);
++	}
++	up_write(&prm_sem);
++	return 0;
++}
++
++int prm_get_cpuid(void)
++{
++	int cpu_id;
++
++	asm("mrc     p15, 0, %0, c0, c0" : "=r" (cpu_id));
++	cpu_id &= 0xfffff000;
++
++	return cpu_id;
++}
++
++static irqreturn_t prm_pmu_handler(int irq, void *dev_id)
++{
++	/*DPRINTK("PMU interrupt generated!\n");
++	 */
++	if (prm_pmu_client)
++		prm_pmu_client->handler(irq, prm_pmu_client->dev_id);
++	return IRQ_HANDLED;
++}
++
++EXPORT_SYMBOL(prm_open_session);
++EXPORT_SYMBOL(prm_close_session);
++EXPORT_SYMBOL(prm_allocate_resource);
++EXPORT_SYMBOL(prm_free_resources);
++EXPORT_SYMBOL(prm_commit_resources);
++EXPORT_SYMBOL(prm_get_cpuid);
++
++/*****************************************************************************/
++/*                                                                           */
++/*                                 PMU    API                                */
++/*                                                                           */
++/*****************************************************************************/
++
++int pmu_read_register(unsigned int client_id, int reg, unsigned int *pval)
++{
++	struct prm_resource *resource;
++	struct prm_client *client;
++	int ret;
++
++	ASSERT_CLIENT_ID(client_id);
++
++	down_read(&prm_sem);
++	client = prm_clients[client_id];
++	if (!client) {
++		up_read(&prm_sem);
++		return -EINVAL;
++	}
++
++	resource = IS_PRM_RESOURCE(reg)? &(prm_resources[PMU_PRM(reg)]):NULL;
++	ret = (get_resource_access(resource) == client);
++	up_read(&prm_sem);
++
++	if (ret)
++		*pval = pmu_read_reg(reg);
++	else
++		return -EACCES;
++
++	return 0;
++}
++
++int pmu_write_register(unsigned int client_id, int reg, unsigned int val)
++{
++	struct prm_resource *resource;
++	struct prm_client *client;
++	int ret;
++
++	ASSERT_CLIENT_ID(client_id);
++
++	down_read(&prm_sem);
++	client = prm_clients[client_id];
++	if (!client) {
++		up_read(&prm_sem);
++		return -EINVAL;
++	}
++
++	resource = IS_PRM_RESOURCE(reg)? &(prm_resources[PMU_PRM(reg)]):NULL;
++	ret = (get_resource_access(resource) == client);
++	up_read(&prm_sem);
++
++	if (ret)
++		pmu_write_reg(reg, val);
++	else
++		return -EACCES;
++
++	return 0;
++}
++
++int pmu_set_event(unsigned int client_id, unsigned int counter,
++		int *pre_type, int type)
++{
++	struct prm_client *client;
++	int ret;
++
++	ASSERT_CLIENT_ID(client_id);
++
++	down_read(&prm_sem);
++	client = prm_clients[client_id];
++	if (!client) {
++		up_read(&prm_sem);
++		return -EINVAL;
++	}
++
++	ret = (client == get_resource_access(NULL)) ;
++	up_read(&prm_sem);
++
++	if (ret) {
++		*pre_type = pmu_select_event(counter, type);
++		if (*pre_type == PMU_EVENT_INVALIDATE)
++			return -EINVAL;
++	}
++	else
++		return -EACCES;
++	return 0;
++}
++
++int pmu_enable_event_counting(unsigned int client_id)
++{
++	struct prm_client *client;
++	unsigned long val;
++	int ret;
++
++	ASSERT_CLIENT_ID(client_id);
++
++	down_read(&prm_sem);
++	client = prm_clients[client_id];
++	if (!client) {
++		up_read(&prm_sem);
++		return -EINVAL;
++	}
++
++	ret = (client == get_resource_access(NULL));
++	up_read(&prm_sem);
++
++	if (ret) {
++		/* enable and reset all counters,
++		 * CCNT counts every clock cycle
++		 */
++		val = 0x07;
++		pmu_write_reg(PMU_PMNC, val);
++	}
++	else
++		return -EACCES;
++	return 0;
++}
++
++int pmu_disable_event_counting(unsigned int client_id)
++{
++	struct prm_client *client;
++	unsigned long val;
++	int ret;
++
++	ASSERT_CLIENT_ID(client_id);
++
++	down_read(&prm_sem);
++	client = prm_clients[client_id];
++	if (!client) {
++		up_read(&prm_sem);
++		return -EINVAL;
++	}
++
++	ret = (client == get_resource_access(NULL));
++	up_read(&prm_sem);
++
++	if (ret) {
++		/* disable all counters */
++		val = 0x10;
++		pmu_write_reg(PMU_PMNC, val);
++	}
++	else
++		return -EACCES;
++	return 0;
++}
++
++int pmu_enable_event_interrupt(unsigned int client_id, int reg)
++{
++	struct prm_client *client;
++	unsigned long val;
++	int ret;
++
++	ASSERT_CLIENT_ID(client_id);
++
++	down_read(&prm_sem);
++	client = prm_clients[client_id];
++	if (!client) {
++		up_read(&prm_sem);
++		return -EINVAL;
++	}
++
++	if (IS_PRM_RESOURCE(reg)) {
++		ret = (get_resource_access(&(prm_resources[PMU_PRM(reg)])) == client);
++		up_read(&prm_sem);
++		if (ret) {
++			val = pmu_read_reg(PMU_INTEN);
++			val |= (0x1 << reg);
++			pmu_write_reg(PMU_INTEN, val);
++		}
++		else
++			return -EACCES;
++	}
++	else {
++		up_read(&prm_sem);
++		return -EINVAL;
++	}
++	return 0;
++}
++
++int pmu_disable_event_interrupt(unsigned int client_id, int reg)
++{
++	struct prm_client *client;
++	unsigned long val;
++	int ret;
++
++	ASSERT_CLIENT_ID(client_id);
++
++	down_read(&prm_sem);
++	client = prm_clients[client_id];
++	if (!client) {
++		up_read(&prm_sem);
++		return -EINVAL;
++	}
++
++	if (IS_PRM_RESOURCE(reg)) {
++		ret = (get_resource_access(&(prm_resources[PMU_PRM(reg)])) == client);
++		up_read(&prm_sem);
++		if (ret) {
++			val = pmu_read_reg(PMU_INTEN);
++			val &= ~(0x1 << reg);
++			pmu_write_reg(PMU_INTEN, val);
++		}
++		else
++			return -EACCES;
++	}
++	else {
++		up_read(&prm_sem);
++		return -EINVAL;
++	}
++	return 0;
++}
++
++int pmu_register_isr(unsigned int client_id,
++		irq_handler_t handler, void *dev_id)
++{
++	struct prm_client *client;
++
++	ASSERT_CLIENT_ID(client_id);
++
++	down_read(&prm_sem);
++	client = prm_clients[client_id];
++	if (!client) {
++		up_read(&prm_sem);
++		return -EINVAL;
++	}
++
++	client->handler = handler;
++	client->dev_id = dev_id;
++	load_isr(client);
++	up_read(&prm_sem);
++	return 0;
++}
++
++int pmu_unregister_isr(unsigned int client_id)
++{
++	struct prm_client *client;
++
++	ASSERT_CLIENT_ID(client_id);
++
++	down_read(&prm_sem);
++	client = prm_clients[client_id];
++	if (!client) {
++		up_read(&prm_sem);
++		return -EINVAL;
++	}
++
++	unload_isr(client);
++	client->handler = NULL;
++	client->dev_id = NULL;
++	up_read(&prm_sem);
++	return 0;
++}
++
++EXPORT_SYMBOL(pmu_read_reg);
++EXPORT_SYMBOL(pmu_write_reg);
++EXPORT_SYMBOL(pmu_read_register);
++EXPORT_SYMBOL(pmu_write_register);
++EXPORT_SYMBOL(pmu_set_event);
++EXPORT_SYMBOL(pmu_enable_event_counting);
++EXPORT_SYMBOL(pmu_disable_event_counting);
++EXPORT_SYMBOL(pmu_enable_event_interrupt);
++EXPORT_SYMBOL(pmu_disable_event_interrupt);
++EXPORT_SYMBOL(pmu_register_isr);
++EXPORT_SYMBOL(pmu_unregister_isr);
++
++/*****************************************************************************/
++/*                                                                           */
++/*                                 COP    API                                */
++/*                                                                           */
++/*****************************************************************************/
++
++int cop_get_num_of_cops(void)
++{
++	return dvfm_op_count();
++}
++
++int cop_get_cop(unsigned int client_id, unsigned int n,
++		struct pxa3xx_fv_info *param)
++{
++	struct op_info *info = NULL;
++	int ret;
++
++	ASSERT_CLIENT_ID(client_id);
++
++	ret = dvfm_get_opinfo(n, &info);
++	if (ret == 0) {
++		md2fvinfo(param, (struct dvfm_md_opt *)info->op);
++	}
++	return ret;
++}
++
++int cop_set_cop(unsigned int client_id, unsigned int n, int mode)
++{
++	struct prm_resource *resource;
++	struct prm_client *client;
++	int ret;
++
++	ASSERT_CLIENT_ID(client_id);
++
++	down_read(&prm_sem);
++	client = prm_clients[client_id];
++	if (!client) {
++		up_read(&prm_sem);
++		return -EINVAL;
++	}
++
++	resource = &prm_resources[PRM_COP];
++	ret = (get_resource_access(resource) == client);
++	up_read(&prm_sem);
++
++	if (ret)
++		return dvfm_request_op(n);
++	return -EACCES;
++}
++
++int cop_get_def_cop(unsigned int client_id, unsigned int *n,
++		struct pxa3xx_fv_info *param)
++{
++	struct op_info *info = NULL;
++	int ret;
++
++	ASSERT_CLIENT_ID(client_id);
++
++	*n = dvfm_get_defop();
++	ret = dvfm_get_opinfo(*n, &info);
++	if (ret == 0) {
++		md2fvinfo(param, (struct dvfm_md_opt *)info->op);
++	}
++	return ret;
++}
++
++int cop_set_def_cop(unsigned int client_id)
++{
++	struct prm_resource *resource;
++	struct prm_client *client;
++	unsigned int def_op;
++	int ret;
++
++	ASSERT_CLIENT_ID(client_id);
++
++	down_read(&prm_sem);
++	client = prm_clients[client_id];
++	if (!client) {
++		up_read(&prm_sem);
++		return -EINVAL;
++	}
++
++	resource = &prm_resources[PRM_COP];
++	ret = (get_resource_access(resource) == client);
++	up_read(&prm_sem);
++
++	def_op = dvfm_get_defop();
++	if (ret)
++		return dvfm_request_op(def_op);
++	return -EACCES;
++}
++
++int cop_get_cur_cop(unsigned int client_id, unsigned int *n,
++		struct pxa3xx_fv_info *param)
++{
++	struct op_info *info = NULL;
++
++	ASSERT_CLIENT_ID(client_id);
++
++	*n = dvfm_get_op(&info);
++	md2fvinfo(param, (struct dvfm_md_opt *)info->op);
++
++	return 0;
++}
++
++EXPORT_SYMBOL(cop_get_num_of_cops);
++EXPORT_SYMBOL(cop_get_cop);
++EXPORT_SYMBOL(cop_set_cop);
++EXPORT_SYMBOL(cop_get_def_cop);
++EXPORT_SYMBOL(cop_set_def_cop);
++EXPORT_SYMBOL(cop_get_cur_cop);
++
++/*****************************************************************************/
++/*                                                                           */
++/*                                 Module Init/Exit                          */
++/*                                                                           */
++/*****************************************************************************/
++
++static int __init prm_init(void)
++{
++	int ret, i , j;
++
++	proc_prm_init();
++	init_rwsem(&prm_sem);
++	/*prm_sem.debug = 1;
++	 */
++	for (i = 0; i < RESOURCE_NUM; i++) {
++		prm_resources[i].access = NULL;
++		prm_resources[i].id = i;
++		proc_add_resource(&prm_resources[i]);
++		for (j = 0; j < MAX_PRIORITIES;j++) {
++		    prm_resources[i].priority[j].resource = &prm_resources[i];
++		    prm_resources[i].priority[j].allocate = NULL;
++		    prm_resources[i].priority[j].active = STATE_UNDEF;
++		    INIT_LIST_HEAD(&(prm_resources[i].priority[j].entry));
++		    proc_add_resource_state(&prm_resources[i].priority[j], j);
++		}
++	}
++
++	for (i = 0; i < MAX_CLIENTS; i++) {
++		prm_clients[i] = NULL;
++	}
++	prm_pmu_client = NULL;
++
++	ret = request_irq(IRQ_PMU, prm_pmu_handler, 0, "PMU", NULL);
++	if (ret < 0) {
++		DPRINTK("PMU interrupt handler registeration: failed!\n");
++		return ret;
++	} else {
++		DPRINTK("PMU interrupt handler registeration: OK!\n");
++	}
++
++	DPRINTK("CPU_ID = 0x%08x\n", prm_get_cpuid());
++
++	return 0;
++}
++
++static void __exit prm_exit(void)
++{
++	int i, j;
++
++	for (i = 0; i < RESOURCE_NUM; i++) {
++		for(j = 0; j < MAX_PRIORITIES;j++) {
++		    proc_del_resource_state(&prm_resources[i].priority[j], j);
++		}
++		proc_del_resource(&prm_resources[i]);
++		memset(&(prm_resources[i]), 0x0, sizeof(struct prm_resource));
++	}
++
++	for (i = 0; i < MAX_CLIENTS; i++) {
++		if (prm_clients[i]) {
++			if (prm_clients[i]->group_cnt) {
++				for (j = 0; j < MAX_GROUPS; j++) {
++					if (prm_clients[i]->groups[j]) {
++					    proc_del_group(prm_clients[i],
++					        prm_clients[i]->groups[j], j);
++					    kfree(prm_clients[i]->groups[j]);
++					}
++				}
++			}
++			proc_del_client(prm_clients[i]);
++			kfree(prm_clients[i]);
++		}
++	}
++	prm_pmu_client = NULL;
++	free_irq(IRQ_PMU, NULL);
++	proc_prm_exit();
++}
++
++module_init(prm_init);
++module_exit(prm_exit);
++
++MODULE_DESCRIPTION("Performance Resources Management");
++MODULE_LICENSE("GPL");
++
+diff -ur linux-2.6.32/arch/arm/mach-pxa/pxa3xx.c kernel/arch/arm/mach-pxa/pxa3xx.c
+--- linux-2.6.32/arch/arm/mach-pxa/pxa3xx.c	2009-12-03 05:51:21.000000000 +0200
++++ kernel/arch/arm/mach-pxa/pxa3xx.c	2009-12-12 16:09:26.482948915 +0200
+@@ -613,3 +613,4 @@
+ }
+ 
+ postcore_initcall(pxa3xx_init);
++
+diff -ur linux-2.6.32/arch/arm/mach-pxa/pxa3xx_dvfm.c kernel/arch/arm/mach-pxa/pxa3xx_dvfm.c
+--- linux-2.6.32/arch/arm/mach-pxa/pxa3xx_dvfm.c	2009-12-13 13:00:35.598610849 +0200
++++ kernel/arch/arm/mach-pxa/pxa3xx_dvfm.c	2009-12-12 16:09:26.482948915 +0200
+@@ -0,0 +1,2319 @@
++/*
++ * PXA3xx DVFM Driver
++ *
++ * Copyright (C) 2007 Marvell Corporation
++ * Haojian Zhuang <haojian.zhuang@marvell.com>
++ *
++ * This software program is licensed subject to the GNU General Public License
++ * (GPL).Version 2,June 1991, available at http://www.fsf.org/copyleft/gpl.html
++
++ * (C) Copyright 2007 Marvell International Ltd.
++ * All Rights Reserved
++ */
++
++#define DEBUG
++
++#include <linux/kernel.h>
++#include <linux/module.h>
++#include <linux/init.h>
++#include <linux/sysdev.h>
++#include <linux/miscdevice.h>
++#include <linux/fs.h>
++#include <linux/workqueue.h>
++#include <linux/delay.h>
++#include <linux/list.h>
++#include <linux/clk.h>
++#include <linux/platform_device.h>
++#include <linux/err.h>
++#include <asm/uaccess.h>
++//#include <asm/arch/pxa-regs.h>
++#include <mach/pxa3xx-regs.h>
++#include <mach/pxa3xx_pmic.h>
++#include <mach/hardware.h>
++#include <mach/dvfm.h>
++#include <mach/pxa3xx_dvfm.h>
++#include <asm/io.h>
++//#include <asm/arch/pxa_ispt.h>
++//#include <asm/arch/mspm_prof.h>
++
++#include "devices.h"
++
++#ifdef CONFIG_CPU_PXA310
++#define FREQ_CORE(xl, xn)	((xl)*(xn)*13)
++
++#define FREQ_SRAM(sflfs)	(((sflfs) == 0x0)?104:	\
++				((sflfs) == 0x1)?156:	\
++				((sflfs) == 0x2)?208:312)
++
++#define FREQ_STMM(smcfs)	(((smcfs) == 0x0)?78:	\
++				((smcfs) == 0x2)?104:	\
++				((smcfs) == 0x5)?208:0)
++
++#define FREQ_DDR(dmcfs)		(((dmcfs) == 0x0)?26:	\
++				((dmcfs) == 0x2)?208:	\
++				((dmcfs) == 0x3)?260:0)
++
++#define FREQ_HSS(hss)		(((hss) == 0x0)?104:	\
++				((hss) == 0x1)?156:	\
++				((hss) == 0x2)?208:0)
++
++#define FREQ_DFCLK(smcfs, df_clkdiv)    				\
++			(((df_clkdiv) == 0x1)?FREQ_STMM((smcfs)):	\
++			((df_clkdiv) == 0x2)?FREQ_STMM((smcfs))/2:	\
++			((df_clkdiv) == 0x3)?FREQ_STMM((smcfs))/4:0)
++
++#define FREQ_EMPICLK(smcfs, empi_clkdiv)				\
++			(((empi_clkdiv) == 0x1)?FREQ_STMM((smcfs)):	\
++			((empi_clkdiv) == 0x2)?FREQ_STMM((smcfs))/2:	\
++			((empi_clkdiv) == 0x3)?FREQ_STMM((smcfs))/4:0)
++
++#define LPJ_PER_MHZ 4988
++#endif
++
++/* Enter D2 before exiting D0CS */
++#define DVFM_LP_SAFE
++
++struct pxa3xx_dvfm_info {
++	/* flags */
++	uint32_t 	       flags;
++
++	/* CPU ID */
++	uint32_t	       cpuid;
++
++	/* LCD clock */
++	struct clk		*lcd_clk;
++
++	/* clock manager register base */
++	unsigned char __iomem	*clkmgr_base;
++
++	/* service power management unit */
++	unsigned char __iomem	*spmu_base;
++
++	/* slave power management unit */
++	unsigned char __iomem	*bpmu_base;
++
++	/* dynamic memory controller register base */
++	unsigned char __iomem	*dmc_base;
++
++	/* static memory controller register base */
++	unsigned char __iomem	*smc_base;
++};
++
++#define MIN_SAFE_FREQUENCY	624
++
++struct info_head pxa3xx_dvfm_op_list = {
++	.list = LIST_HEAD_INIT(pxa3xx_dvfm_op_list.list),
++	.lock = RW_LOCK_UNLOCKED,
++};
++
++#ifdef CONFIG_PXA3xx_DVFM_STATS
++
++static unsigned int switch_lowpower_before, switch_lowpower_after;
++
++static int pxa3xx_stats_notifier_freq(struct notifier_block *nb,
++				unsigned long val, void *data);
++static struct notifier_block notifier_freq_block = {
++	.notifier_call = pxa3xx_stats_notifier_freq,
++};
++#endif
++
++/* the operating point preferred by policy maker or user */
++static int preferred_op;
++static int current_op;
++
++extern unsigned int cur_op;		/* current operating point */
++extern unsigned int def_op;		/* default operating point */
++
++extern int enter_d0cs_a(volatile u32 *, volatile u32 *);
++extern int exit_d0cs_a(volatile u32 *, volatile u32 *);
++extern int md2fvinfo(struct pxa3xx_fv_info *, struct dvfm_md_opt *);
++extern void set_idle_op(int, int);
++
++#ifdef CONFIG_FB_PXA
++extern void pxafb_set_pcd(void);
++#else
++static void pxafb_set_pcd(void) {}
++#endif
++
++static int dvfm_dev_id;
++#define LPJ_D0CS (293888 * 100 / HZ)
++#define LPJ_104M (517120 * 100 / HZ)
++#define LPJ_156M (778128 * 100 / HZ)
++#define LPJ_208M (1036288 * 100 / HZ)
++#define LPJ_416M (2076672 * 100 / HZ)
++#define LPJ_624M (3112960 * 100 / HZ)
++#define LPJ_806M (4020906 * 100 / HZ)
++
++static int d0cs_lpj = LPJ_D0CS;
++
++static int boot_core_freq = 0;
++
++int out_d0cs = 0;
++
++/* define the operating point of S0D0 and S0D0CS mode */
++static struct dvfm_md_opt pxa300_op_array[] = {
++	/* 60MHz -- ring oscillator */
++	{
++		.vcc_core = 1000,
++		.vcc_sram = 1100,
++		.xl = 0,
++		.xn = 0,
++		.smcfs = 15,
++		.sflfs = 60,
++		.hss = 60,
++		.dmcfs = 30, /* will be 60MHZ for PXA310 A2 and PXA935/PXA940 */
++		.df_clk = 15,
++		.empi_clk = 15,
++		.power_mode = POWER_MODE_D0CS,
++		.flag = OP_FLAG_FACTORY,
++		.lpj = 293888*100/HZ,
++		.name = "D0CS",
++	},
++	/* 104MHz */
++	{
++		.vcc_core = 1000,
++		.vcc_sram = 1100,
++		.xl = 8,
++		.xn = 1,
++		.smcfs = 78,
++		.sflfs = 104,
++		.hss = 104,
++		.dmcfs = 260,
++		/* Actually it's 19.5, not 19 */
++		.df_clk = 19,
++		.empi_clk = 19,
++		.power_mode = POWER_MODE_D0,
++		.flag = OP_FLAG_FACTORY,
++		.lpj = 517120*100/HZ,
++		.name = "104M",
++	},
++	/* 208MHz */
++	{
++		.vcc_core = 1000,
++		.vcc_sram = 1100,
++		.xl = 16,
++		.xn = 1,
++		.smcfs = 104,
++		.sflfs = 156,
++		.hss = 104,
++		.dmcfs = 260,
++		.df_clk = 52,
++		.empi_clk = 52,
++		.power_mode = POWER_MODE_D0,
++		.flag = OP_FLAG_FACTORY,
++		.lpj = 1036288*100/HZ,
++		.name = "208M",
++	},
++	/* 416MHz */
++	{
++		.vcc_core = 1100,
++		.vcc_sram = 1200,
++		.xl = 16,
++		.xn = 2,
++		.smcfs = 104,
++		.sflfs = 208,
++		.hss = 156,
++		.dmcfs = 260,
++		.df_clk = 52,
++		.empi_clk = 52,
++		.power_mode = POWER_MODE_D0,
++		.flag = OP_FLAG_FACTORY,
++		.lpj = 2076672*100/HZ,
++		.name = "416M",
++	},
++	/* 624MHz */
++	{
++		.vcc_core = 1375,
++		.vcc_sram = 1400,
++		.xl = 24,
++		.xn = 2,
++		.smcfs = 208,
++		.sflfs = 312,
++		.hss = 208,
++		.dmcfs = 260,
++		.df_clk = 52,
++		.empi_clk = 52,
++		.power_mode = POWER_MODE_D0,
++		.flag = OP_FLAG_FACTORY,
++		.lpj = 3112960*100/HZ,
++		.name = "624M",
++	},
++	/* D1 mode */
++	{
++		.vcc_core = 1100,
++		.vcc_sram = 1200,
++		.power_mode = POWER_MODE_D1,
++		.flag = OP_FLAG_FACTORY,
++		.name = "D1",
++	},
++	/* D2 mode */
++	{
++		.vcc_core = 1100,
++		.vcc_sram = 1200,
++		.power_mode = POWER_MODE_D2,
++		.flag = OP_FLAG_FACTORY,
++		.name = "D2",
++	},
++};
++
++static struct dvfm_md_opt pxa320_op_array[] = {
++	/* 60MHz -- ring oscillator */
++	{
++		.vcc_core = 1000,
++		.vcc_sram = 1100,
++		.xl = 0,
++		.xn = 0,
++		.smcfs = 15,
++		.sflfs = 60,
++		.hss = 60,
++		.dmcfs = 30,
++		.df_clk = 15,
++		.empi_clk = 15,
++		.power_mode = POWER_MODE_D0CS,
++		.flag = OP_FLAG_FACTORY,
++		.lpj = 293888*100/HZ,
++		.name = "D0CS",
++	},
++	/* 104MHz */
++	{
++		.vcc_core = 1000,
++		.vcc_sram = 1100,
++		.xl = 8,
++		.xn = 1,
++		.smcfs = 78,
++		.sflfs = 104,
++		.hss = 104,
++		.dmcfs = 260,
++		/* Actually it's 19.5, not 19 */
++		.df_clk = 19,
++		.empi_clk = 19,
++		.power_mode = POWER_MODE_D0,
++		.flag = OP_FLAG_FACTORY,
++		.lpj = 517120*100/HZ,
++		.name = "104M",
++	},
++	/* 208MHz */
++	{
++		.vcc_core = 1000,
++		.vcc_sram = 1100,
++		.xl = 16,
++		.xn = 1,
++		.smcfs = 104,
++		.sflfs = 156,
++		.hss = 104,
++		.dmcfs = 260,
++		.df_clk = 52,
++		.empi_clk = 52,
++		.power_mode = POWER_MODE_D0,
++		.flag = OP_FLAG_FACTORY,
++		.lpj = 1036288*100/HZ,
++		.name = "208M",
++	},
++	/* 416MHz */
++	{
++		.vcc_core = 1100,
++		.vcc_sram = 1200,
++		.xl = 16,
++		.xn = 2,
++		.smcfs = 104,
++		.sflfs = 208,
++		.hss = 156,
++		.dmcfs = 260,
++		.df_clk = 52,
++		.empi_clk = 52,
++		.power_mode = POWER_MODE_D0,
++		.flag = OP_FLAG_FACTORY,
++		.lpj = 2076672*100/HZ,
++		.name = "416M",
++	},
++	/* 624MHz */
++	{
++		.vcc_core = 1375,
++		.vcc_sram = 1400,
++		.xl = 24,
++		.xn = 2,
++		.smcfs = 208,
++		.sflfs = 312,
++		.hss = 208,
++		.dmcfs = 260,
++		.df_clk = 52,
++		.empi_clk = 52,
++		.power_mode = POWER_MODE_D0,
++		.flag = OP_FLAG_FACTORY,
++		.lpj = 3112960*100/HZ,
++		.name = "624M",
++	},
++	/* 806MHz */
++	{
++		.vcc_core = 1400,
++		.vcc_sram = 1400,
++		.xl = 31,
++		.xn = 2,
++		.smcfs = 208,
++		.sflfs = 312,
++		.hss = 208,
++		.dmcfs = 260,
++		.df_clk = 52,
++		.empi_clk = 52,
++		.power_mode = POWER_MODE_D0,
++		.flag = OP_FLAG_FACTORY,
++		.lpj = 4020906*100/HZ,
++		.name = "806M",
++	},
++#if 0
++	/* D1 mode */
++	{
++		.vcc_core = 1100,
++		.vcc_sram = 1200,
++		.power_mode = POWER_MODE_D1,
++		.flag = OP_FLAG_FACTORY,
++		.name = "D1",
++	},
++#endif
++	/* D2 mode */
++	{
++		.vcc_core = 1100,
++		.vcc_sram = 1200,
++		.power_mode = POWER_MODE_D2,
++		.flag = OP_FLAG_FACTORY,
++		.name = "D2",
++	},
++};
++
++static struct dvfm_md_opt pxa930_op_array[] = {
++	/* 60MHz -- ring oscillator */
++	{
++		.vcc_core = 1000,
++		.vcc_sram = 1100,
++		.xl = 0,
++		.xn = 0,
++		.smcfs = 15,
++		.sflfs = 60,
++		.hss = 60,
++		.dmcfs = 30,
++		.df_clk = 15,
++		.empi_clk = 15,
++		.power_mode = POWER_MODE_D0CS,
++		.flag = OP_FLAG_FACTORY,
++		.lpj = 293888*100/HZ,
++		.name = "D0CS",
++	},
++	/* 156MHz -- single PLL mode */
++	{
++		.vcc_core = 1000,
++		.vcc_sram = 1100,
++		.xl = 12,
++		.xn = 1,
++		.smcfs = 104,
++		.sflfs = 156,
++		.hss = 104,
++		.dmcfs = 208,
++		.df_clk = 52,
++		.empi_clk = 52,
++		.power_mode = POWER_MODE_D0,
++		.flag = OP_FLAG_FACTORY,
++		.lpj = 778128*100/HZ,
++		.name = "156M",
++	},
++	/* 208MHz */
++	{
++		.vcc_core = 1000,
++		.vcc_sram = 1100,
++		.xl = 16,
++		.xn = 1,
++		.smcfs = 104,
++		.sflfs = 156,
++		.hss = 104,
++		.dmcfs = 260,
++		.df_clk = 52,
++		.empi_clk = 52,
++		.power_mode = POWER_MODE_D0,
++		.flag = OP_FLAG_FACTORY,
++		.lpj = 1036288*100/HZ,
++		.name = "208M",
++	},
++	/* 416MHz */
++	{
++		.vcc_core = 1100,
++		.vcc_sram = 1200,
++		.xl = 16,
++		.xn = 2,
++		.smcfs = 104,
++		.sflfs = 208,
++		.hss = 156,
++		.dmcfs = 260,
++		.df_clk = 52,
++		.empi_clk = 52,
++		.power_mode = POWER_MODE_D0,
++		.flag = OP_FLAG_FACTORY,
++		.lpj = 2076672*100/HZ,
++		.name = "416M",
++	},
++	/* 624MHz */
++	{
++		.vcc_core = 1375,
++		.vcc_sram = 1400,
++		.xl = 24,
++		.xn = 2,
++		.smcfs = 208,
++		.sflfs = 312,
++		.hss = 208,
++		.dmcfs = 260,
++		.df_clk = 52,
++		.empi_clk = 52,
++		.power_mode = POWER_MODE_D0,
++		.flag = OP_FLAG_FACTORY,
++		.lpj = 3112960*100/HZ,
++		.name = "624M",
++	},
++	/* D2 mode */
++	{
++		.vcc_core = 1100,
++		.vcc_sram = 1200,
++		.power_mode = POWER_MODE_D2,
++		.flag = OP_FLAG_FACTORY,
++		.name = "D2",
++	},
++};
++
++static struct dvfm_md_opt pxa935_op_array[] = {
++	/* 60MHz -- ring oscillator */
++	{
++		.vcc_core = 1250,
++		.xl = 0,
++		.xn = 0,
++		.smcfs = 15,
++		.sflfs = 60,
++		.hss = 60,
++		.dmcfs = 30,
++		.df_clk = 15,
++		.empi_clk = 15,
++		.power_mode = POWER_MODE_D0CS,
++		.flag = OP_FLAG_FACTORY,
++		.lpj = 293888*100/HZ,
++		.name = "D0CS",
++	},
++	/* 156MHz -- single PLL mode */
++	{
++		.vcc_core = 1250,
++		.xl = 12,
++		.xn = 1,
++		.smcfs = 104,
++		.sflfs = 156,
++		.hss = 104,
++		.dmcfs = 208,
++		.df_clk = 52,
++		.empi_clk = 52,
++		.power_mode = POWER_MODE_D0,
++		.flag = OP_FLAG_FACTORY,
++		.lpj = 778128*100/HZ,
++		.name = "156M",
++	},
++	/* 208MHz */
++	{
++		.vcc_core = 1250,
++		.xl = 16,
++		.xn = 1,
++		.smcfs = 104,
++		.sflfs = 156,
++		.hss = 104,
++		.dmcfs = 260,
++		.df_clk = 52,
++		.empi_clk = 52,
++		.power_mode = POWER_MODE_D0,
++		.flag = OP_FLAG_FACTORY,
++		.lpj = 1036288*100/HZ,
++		.name = "208M",
++	},
++	/* 416MHz */
++	{
++		.vcc_core = 1250,
++		.xl = 16,
++		.xn = 2,
++		.smcfs = 104,
++		.sflfs = 208,
++		.hss = 156,
++		.dmcfs = 260,
++		.df_clk = 52,
++		.empi_clk = 52,
++		.power_mode = POWER_MODE_D0,
++		.flag = OP_FLAG_FACTORY,
++		.lpj = 2076672*100/HZ,
++		.name = "416M",
++	},
++	/* 624MHz */
++	{
++		.vcc_core = 1250,
++		.xl = 24,
++		.xn = 2,
++		.smcfs = 208,
++		.sflfs = 312,
++		.hss = 208,
++		.dmcfs = 260,
++		.df_clk = 52,
++		.empi_clk = 52,
++		.power_mode = POWER_MODE_D0,
++		.flag = OP_FLAG_FACTORY,
++		.lpj = 3112960*100/HZ,
++		.name = "624M",
++	},
++#if 0
++	/* D1 mode */
++	{
++		.vcc_core = 1250,
++		.power_mode = POWER_MODE_D1,
++		.flag = OP_FLAG_FACTORY,
++		.name = "D1",
++	},
++#endif
++	/* D2 mode */
++	{
++		.vcc_core = 1250,
++		.power_mode = POWER_MODE_D2,
++		.flag = OP_FLAG_FACTORY,
++		.name = "D2",
++	},
++	/* CG (clock gated) mode */
++	{
++		.vcc_core = 1250,
++		.power_mode = POWER_MODE_CG,
++		.flag = OP_FLAG_FACTORY,
++		.name = "CG",
++	},
++
++};
++
++struct proc_op_array {
++	unsigned int cpuid;
++	char	*cpu_name;
++	struct dvfm_md_opt *op_array;
++	unsigned int nr_op;
++};
++
++#define ARRAY_AND_SIZE(x)	(x), ARRAY_SIZE(x)
++static struct proc_op_array proc_op_arrays[] = {
++	{0x6880, "PXA300", ARRAY_AND_SIZE(pxa300_op_array)},
++	{0x6890, "PXA310", ARRAY_AND_SIZE(pxa300_op_array)},
++	{0x6820, "PXA320", ARRAY_AND_SIZE(pxa320_op_array)},
++	{0x6830, "PXA930", ARRAY_AND_SIZE(pxa930_op_array)},
++	{0x6930, "PXA935/PXA940", ARRAY_AND_SIZE(pxa935_op_array)},
++};
++
++extern void pxa_clkcfg_write(unsigned int);
++
++static int prepare_dmc(void *driver_data, int flag);
++static int polling_dmc(void *driver_data);
++
++#ifdef CONFIG_ISPT
++static int ispt_dvfm_op(int old, int new)
++{
++	return ispt_dvfm_msg(old, new);
++}
++
++static int ispt_block_dvfm(int enable, int dev_id)
++{
++	int ret;
++	if (enable)
++		ret = ispt_driver_msg(CT_P_DVFM_BLOCK_REQ, dev_id);
++	else
++		ret = ispt_driver_msg(CT_P_DVFM_BLOCK_REL, dev_id);
++	return ret;
++}
++
++static int ispt_power_state_d2(void)
++{
++	return ispt_power_msg(CT_P_PWR_STATE_ENTRY_D2);
++}
++#else
++static int ispt_dvfm_op(int old, int new) { return 0; }
++static int ispt_block_dvfm(int enable, int dev_id) { return 0; }
++static int ispt_power_state_d2(void) { return 0; }
++#endif
++
++unsigned int pxa3xx_clk_to_lpj(unsigned int clk)
++{
++        if (clk == 624000000)
++                return LPJ_624M;
++        if (clk == 416000000)
++                return LPJ_416M;
++        if (clk == 208000000)
++                return LPJ_208M;
++        if (clk == 156000000)
++                return LPJ_156M;
++        if (clk == 104000000)
++                return LPJ_104M;
++	if (clk == 60000000) 
++		return LPJ_D0CS;
++	
++	printk(KERN_CRIT "%s does not support clk (%d MHz)\n", 
++		__FILE__, clk/1000000);
++
++	return 0; 
++}
++
++/* #####################Debug Function######################## */
++static int dump_op(void *driver_data, struct op_info *p, char *buf)
++{
++	int len, count, x;
++	struct dvfm_md_opt *q = (struct dvfm_md_opt *)p->op;
++
++	if (q == NULL)
++		len = sprintf(buf, "Can't dump the op info\n");
++	else {
++		/* calculate how much bits is set in device word */
++		x = p->device;
++		for (count = 0; x; x = x & (x - 1), count++);
++		len = sprintf(buf, "OP:%d name:%s [%s, %d]\n",
++				p->index, q->name, (count)?"Disabled"
++				:"Enabled", count);
++		len += sprintf(buf + len, "vcore:%d vsram:%d xl:%d xn:%d "
++				"smcfs:%d sflfs:%d hss:%d dmcfs:%d df_clk:%d "
++				"power_mode:%d flag:%d\n",
++				q->vcc_core, q->vcc_sram, q->xl, q->xn,
++				q->smcfs, q->sflfs, q->hss, q->dmcfs,
++				q->df_clk, q->power_mode, q->flag);
++	}
++	return len;
++}
++
++static int dump_op_list(void *driver_data, struct info_head *op_table, int flag)
++{
++	struct op_info *p = NULL;
++	struct dvfm_md_opt *q = NULL;
++	struct list_head *list = NULL;
++	struct pxa3xx_dvfm_info *info = driver_data;
++	char buf[256];
++
++	if (!op_table || list_empty(&op_table->list)) {
++		printk(KERN_WARNING "op list is null\n");
++		return -EINVAL;
++	}
++	memset(buf, 0, 256);
++	list_for_each(list, &op_table->list) {
++		p = list_entry(list, struct op_info, list);
++		q = (struct dvfm_md_opt *)p->op;
++		if (q->flag <= flag) {
++			dump_op(info, p, buf);
++			pr_debug("%s", buf);
++		}
++	}
++	return 0;
++}
++
++/* ########################################################## */
++static int freq2reg(struct pxa3xx_fv_info *fv_info, struct dvfm_md_opt *orig)
++{
++	int res = -EFAULT, tmp;
++
++	if (orig && fv_info) {
++		fv_info->vcc_core = orig->vcc_core;
++		fv_info->vcc_sram = orig->vcc_sram;
++		if (orig->power_mode == POWER_MODE_D0) {
++			res = 0;
++			fv_info->xl = orig->xl;
++			fv_info->xn = orig->xn;
++			fv_info->d0cs = 0;
++			if (orig->smcfs == 78)
++				fv_info->smcfs = 0;
++			else if (orig->smcfs == 104)
++				fv_info->smcfs = 2;
++			else if (orig->smcfs == 208)
++				fv_info->smcfs = 5;
++			else
++				res = -EINVAL;
++			if (orig->sflfs == 104)
++				fv_info->sflfs = 0;
++			else if (orig->sflfs == 156)
++				fv_info->sflfs = 1;
++			else if (orig->sflfs == 208)
++				fv_info->sflfs = 2;
++			else if (orig->sflfs == 312)
++				fv_info->sflfs = 3;
++			else
++				res = -EINVAL;
++			if (orig->hss == 104)
++				fv_info->hss = 0;
++			else if (orig->hss == 156)
++				fv_info->hss = 1;
++			else if (orig->hss == 208)
++				fv_info->hss = 2;
++			else
++				res = -EINVAL;
++			if (orig->dmcfs == 26)
++				fv_info->dmcfs = 0;
++			else if (orig->dmcfs == 208)
++				fv_info->dmcfs = 2;
++			else if (orig->dmcfs == 260)
++				fv_info->dmcfs = 3;
++			else
++				res = -EINVAL;
++			tmp = orig->smcfs / orig->df_clk;
++			if (tmp == 2)
++				fv_info->df_clk = 2;
++			else if (tmp == 4)
++				fv_info->df_clk = 3;
++			fv_info->empi_clk = fv_info->df_clk;
++		} else if (orig->power_mode == POWER_MODE_D0CS) {
++			fv_info->d0cs = 1;
++			res = 0;
++		}
++	}
++	return res;
++}
++
++int md2fvinfo(struct pxa3xx_fv_info *fv_info, struct dvfm_md_opt *orig)
++{
++	return freq2reg(fv_info, orig);
++}
++
++static int reg2freq(void *driver_data, struct dvfm_md_opt *fv_info)
++{
++	struct pxa3xx_dvfm_info *info = driver_data;
++	int res = -EFAULT, tmp;
++	uint32_t accr;
++
++	if (fv_info) {
++		res = 0;
++		if (fv_info->power_mode == POWER_MODE_D0CS) {
++			/* set S0D0CS operating pointer */
++			fv_info->power_mode = POWER_MODE_D0CS;
++			fv_info->xl = 0;
++			fv_info->xn = 0;
++			fv_info->smcfs = 15;
++			fv_info->sflfs = 60;
++			fv_info->hss = 60;
++			/* PXA310 A2 or PXA935/PXA940 */
++			accr = __raw_readl(info->clkmgr_base + ACCR_OFF);
++			if (accr & 0x80)
++				fv_info->dmcfs = 60;
++			else
++				fv_info->dmcfs = 30;
++			fv_info->df_clk = 15;
++			fv_info->empi_clk = 15;
++		} else {
++			/* set S0D0 operating pointer */
++			fv_info->power_mode = POWER_MODE_D0;
++			tmp = fv_info->smcfs;
++			if (tmp == 0)
++				fv_info->smcfs = 78;
++			else if (tmp == 2)
++				fv_info->smcfs = 104;
++			else if (tmp == 5)
++				fv_info->smcfs = 208;
++			else
++				res = -EINVAL;
++			tmp = fv_info->sflfs;
++			if (tmp == 0)
++				fv_info->sflfs = 104;
++			else if (tmp == 1)
++				fv_info->sflfs = 156;
++			else if (tmp == 2)
++				fv_info->sflfs = 208;
++			else if (tmp == 3)
++				fv_info->sflfs = 312;
++			tmp = fv_info->hss;
++			if (tmp == 0)
++				fv_info->hss = 104;
++			else if (tmp == 1)
++				fv_info->hss = 156;
++			else if (tmp == 2)
++				fv_info->hss = 208;
++			else
++				res = -EINVAL;
++			tmp = fv_info->dmcfs;
++			if (tmp == 0)
++				fv_info->dmcfs = 26;
++			else if (tmp == 2)
++				fv_info->dmcfs = 208;
++			else if (tmp == 3)
++				fv_info->dmcfs = 260;
++			else
++				res = -EINVAL;
++			tmp = fv_info->df_clk;
++			if (tmp == 1)
++				fv_info->df_clk = fv_info->smcfs;
++			else if (tmp == 2)
++				fv_info->df_clk = fv_info->smcfs / 2;
++			else if (tmp == 3)
++				fv_info->df_clk = fv_info->smcfs / 4;
++			fv_info->empi_clk = fv_info->df_clk;
++		}
++	}
++	return res;
++}
++
++/* Get current setting, and record it in fv_info structure
++ */
++static int capture_op_info(void *driver_data, struct dvfm_md_opt *fv_info)
++{
++	struct pxa3xx_dvfm_info *info = driver_data;
++	int res = -EFAULT;
++	uint32_t acsr, memclkcfg;
++
++	if (fv_info) {
++		memset(fv_info, 0, sizeof(struct dvfm_md_opt));
++		acsr = __raw_readl(info->clkmgr_base + ACSR_OFF);
++		fv_info->xl = (acsr >> ACCR_XL_OFFSET) & 0x1F;
++		fv_info->xn = (acsr >> ACCR_XN_OFFSET) & 0x07;
++		fv_info->smcfs = (acsr >> ACCR_SMCFS_OFFSET) & 0x07;
++		fv_info->sflfs = (acsr >> ACCR_SFLFS_OFFSET) & 0x03;
++		fv_info->hss = (acsr >> ACCR_HSS_OFFSET) & 0x03;
++		fv_info->dmcfs = (acsr >> ACCR_DMCFS_OFFSET) & 0x03;
++		fv_info->power_mode = (acsr >> ACCR_D0CS_OFFSET) & 0x01;
++		memclkcfg = __raw_readl(info->smc_base + MEMCLKCFG_OFF);
++		fv_info->df_clk = (memclkcfg >> MEMCLKCFG_DF_OFFSET) & 0x07;
++		fv_info->empi_clk = (memclkcfg >> MEMCLKCFG_EMPI_OFFSET) & 0x07;
++		res = reg2freq(info, fv_info);
++		pxa3xx_pmic_get_voltage(VCC_CORE, &fv_info->vcc_core);
++		if ((info->cpuid & 0xFFF0) == 0x6930) {
++			/* PXA935/PXA940 doesn't have VCC_SRAM */
++			fv_info->vcc_sram = 0;
++		} else {
++			pxa3xx_pmic_get_voltage(VCC_SRAM, &fv_info->vcc_sram);
++		}
++		/* TODO: mix up the usage of struct dvfm_md_opt and struct pxa3xx_fv_info
++		 * better to define reg2freq(struct dvfm_md_opt *md_info,
++		 * struct pxa3xx_fv_info *fv_info)
++		 */
++	}
++	return res;
++}
++
++/* return all op including user defined op, and boot op */
++static int get_op_num(void *driver_data, struct info_head *op_table)
++{
++	struct list_head *entry = NULL;
++	int num = 0;
++
++	if (!op_table)
++		goto out;
++	read_lock(&op_table->lock);
++	if (list_empty(&op_table->list)) {
++		read_unlock(&op_table->lock);
++		goto out;
++	}
++	list_for_each(entry, &op_table->list) {
++		num++;
++	}
++	read_unlock(&op_table->lock);
++out:
++	return num;
++}
++
++/* return op name. */
++static char *get_op_name(void *driver_data, struct op_info *p)
++{
++	struct dvfm_md_opt *q = NULL;
++	if (p == NULL)
++		return NULL;
++	q = (struct dvfm_md_opt *)p->op;
++	return q->name;
++}
++
++static int update_voltage(void *driver_data, struct dvfm_md_opt *old, struct dvfm_md_opt *new)
++{
++	struct pxa3xx_dvfm_info *info = driver_data;
++
++	if (!(info->flags & PXA3xx_USE_POWER_I2C)) {
++		pxa3xx_pmic_set_voltage(VCC_CORE, new->vcc_core);
++		pxa3xx_pmic_set_voltage(VCC_SRAM, new->vcc_sram);
++	}
++	return 0;
++}
++
++static void pxa3xx_enter_d0cs(void *driver_data)
++{
++	struct pxa3xx_dvfm_info *info = driver_data;
++
++	unsigned int reg, spll = 0;
++	uint32_t accr, mdrefr;
++
++	reg = (12 << ACCR_XL_OFFSET) | (1 << ACCR_XN_OFFSET);
++	accr = __raw_readl(info->clkmgr_base + ACCR_OFF);
++	if (reg == (accr & (ACCR_XN_MASK | ACCR_XL_MASK))) {
++		spll = 1;
++	}
++	/* clk_disable(info->lcd_clk);*/
++	enter_d0cs_a((volatile u32 *)info->clkmgr_base, (volatile u32 *)info->dmc_base);
++	pxafb_set_pcd();
++	/* clk_enable(info->lcd_clk);*/
++	/* update to D0CS LPJ, it must be updated before udelay() */
++	loops_per_jiffy = d0cs_lpj;
++	if (cpu_is_pxa930())
++		udelay(200);
++	else
++		udelay(100);
++
++	/* disable PLL */
++	accr = __raw_readl(info->clkmgr_base + ACCR_OFF);
++	if (spll) {
++		/* single PLL mode only disable System PLL */
++		accr |= (1 << ACCR_SPDIS_OFFSET);
++	} else {
++		/* Disable both System PLL and Core PLL */
++		accr |= (1 << ACCR_XPDIS_OFFSET) | (1 << ACCR_SPDIS_OFFSET);
++	}
++	__raw_writel(accr, info->clkmgr_base + ACCR_OFF);
++
++	mdrefr = __raw_readl(info->dmc_base + MDREFR_OFF);
++	__raw_writel(mdrefr, info->dmc_base + MDREFR_OFF);
++}
++
++static void pxa3xx_exit_d0cs(void *driver_data)
++{
++	struct pxa3xx_dvfm_info *info = driver_data;
++	unsigned int spll = 0;
++	uint32_t reg, accr, acsr, mdrefr;
++
++	reg = (12 << ACCR_XL_OFFSET) | (1 << ACCR_XN_OFFSET);
++	accr = __raw_readl(info->clkmgr_base + ACCR_OFF);
++	if (reg == (accr & (ACCR_XN_MASK | ACCR_XL_MASK))) {
++		spll = 1;
++	}
++	/* enable PLL */
++	if (spll) {
++		/* single PLL mode only enable System PLL */
++		accr = __raw_readl(info->clkmgr_base + ACCR_OFF);
++		accr &= ~(1 << ACCR_SPDIS_OFFSET);
++		__raw_writel(accr, info->clkmgr_base + ACCR_OFF);
++		do {
++			acsr = __raw_readl(info->clkmgr_base + ACSR_OFF);
++		} while (acsr & (1 << ACCR_SPDIS_OFFSET));
++	} else {
++		/* enable both System PLL and Core PLL */
++		accr = __raw_readl(info->clkmgr_base + ACCR_OFF);
++		accr &= ~((1 << ACCR_XPDIS_OFFSET) |
++				(1 << ACCR_SPDIS_OFFSET));
++		__raw_writel(accr, info->clkmgr_base + ACCR_OFF);
++		do {
++			acsr = __raw_readl(info->clkmgr_base + ACSR_OFF);
++		} while (acsr & (1 << ACCR_XPDIS_OFFSET)
++			|| acsr & (1 << ACCR_SPDIS_OFFSET));
++	}
++
++	/* clk_disable(info->lcd_clk);*/
++	exit_d0cs_a((volatile u32 *)info->clkmgr_base, (volatile u32 *)info->dmc_base);
++	mdrefr = __raw_readl(info->dmc_base + MDREFR_OFF);
++	__raw_writel(mdrefr, info->dmc_base + MDREFR_OFF);
++	pxafb_set_pcd();
++	/* clk_enable(info->lcd_clk);*/
++}
++
++/* Return 1 if Grayback PLL is on. */
++static int check_grayback_pll(void *driver_data)
++{
++	struct pxa3xx_dvfm_info *info = driver_data;
++
++	return (__raw_readl(info->clkmgr_base + OSCC_OFF) & (1 << 17));
++}
++
++static int set_grayback_pll(void *driver_data, int lev)
++{
++	struct pxa3xx_dvfm_info *info = driver_data;
++	int timeout = 100, turnoff;
++	uint32_t oscc, agenp;
++
++	if ((info->cpuid & 0xFFF0) != 0x6830 && (info->cpuid & 0xFFF0) != 0x6930) {
++		/* It's not PXA930/PXA935/PXA940*/
++		return 0;
++	}
++	if (lev) {
++		/* turn on grayback PLL */
++		for (;;){
++			timeout = 100;
++			/* clear OSCC[GPRM] */
++			oscc = __raw_readl(info->clkmgr_base + OSCC_OFF);
++			oscc &= ~(1 << 18);
++			__raw_writel(oscc, info->clkmgr_base + OSCC_OFF);
++
++			/* set AGENP[GBPLL_CTRL] and AGENP[GBPLL_ST] */
++			agenp = __raw_readl(info->bpmu_base + AGENP_OFF);
++			agenp |= (3 << 28);
++			__raw_writel(agenp, info->bpmu_base + AGENP_OFF);
++
++			/* check OSCC[GPRL] */
++			do {
++				oscc = __raw_readl(info->clkmgr_base + OSCC_OFF);
++				if (--timeout == 0)
++					break;
++			} while (!(oscc & (1 << 17)));
++
++			if (timeout)
++				break;
++		}
++	} else {
++		/* turn off Grayback PLL */
++		for (;;){
++			timeout = 100;
++			/* clear AGENP[GBPLL_CTRL] and AGENP[GBPLL_ST] */
++			agenp = __raw_readl(info->bpmu_base + AGENP_OFF);
++			if (agenp & (1 << 28)) {
++				turnoff = 1;
++				agenp &= ~(3 << 28);
++				agenp |= (2 << 28);
++				__raw_writel(agenp, info->bpmu_base + AGENP_OFF);
++
++				/* check OSCC[GPRL] */
++				do {
++					oscc = __raw_readl(info->clkmgr_base + OSCC_OFF);
++					if (--timeout == 0)
++						break;
++				} while ((oscc & (1 << 17)));
++			}
++
++			if (timeout)
++				break;
++		}
++		if (turnoff) {
++			/* set OSCC[GPRM] */
++			oscc = __raw_readl(info->clkmgr_base + OSCC_OFF);
++			oscc |= (1 << 18);
++			__raw_writel(oscc, info->clkmgr_base + OSCC_OFF);
++		}
++	}
++	return 0;
++}
++
++/*
++ * Return 2 if MTS should be changed to 2.
++ * Return 1 if MTS should be changed to 1.
++ * Return 0 if MTS won't be changed.
++ * In this function, the maxium MTS is 2.
++ */
++static int check_mts(struct dvfm_md_opt *old, struct dvfm_md_opt *new)
++{
++	int ret = 0;
++	if ((old->xn == 1) && (new->xn == 2))
++		ret = 2;
++	if ((old->xn == 2) && (new->xn == 1))
++		ret = 1;
++	return ret;
++}
++
++static int set_mts(void *driver_data, int mts)
++{
++	struct pxa3xx_dvfm_info *info = driver_data;
++	unsigned int ascr;
++
++	ascr = __raw_readl(info->bpmu_base + ASCR_OFF);
++	ascr &= ~(3 << ASCR_MTS_OFFSET);
++	ascr |= (mts << ASCR_MTS_OFFSET);
++	__raw_writel(ascr, info->bpmu_base + ASCR_OFF);
++
++	/* wait MTS is set */
++	do {
++		ascr = __raw_readl(info->bpmu_base + ASCR_OFF);
++	}while (((ascr >> ASCR_MTS_OFFSET) & 0x3)
++		!= ((ascr >> ASCR_MTS_S_OFFSET) & 0x3));
++
++	return 0;
++}
++
++static int prepare_dmc(void *driver_data, int flag)
++{
++	struct pxa3xx_dvfm_info *info = driver_data;
++	int pll;
++	uint32_t mdcnfg, ddr_hcal;
++
++	if (flag == DMEMC_D0CS_ENTER) {
++		mdcnfg = __raw_readl(info->dmc_base + MDCNFG_OFF);
++		mdcnfg |= (1 << MDCNFG_HWFREQ_OFFSET);
++		__raw_writel(mdcnfg, info->dmc_base + MDCNFG_OFF);
++
++		ddr_hcal = __raw_readl(info->dmc_base + DDR_HCAL_OFF);
++		ddr_hcal &= ~(1 << HCAL_HCEN_OFFSET);
++		__raw_writel(ddr_hcal, info->dmc_base + DDR_HCAL_OFF);
++
++		return 0;
++	} else if (flag == DMEMC_D0CS_EXIT) {
++		mdcnfg = __raw_readl(info->dmc_base + MDCNFG_OFF);
++		mdcnfg |= (1 << MDCNFG_HWFREQ_OFFSET);
++		__raw_writel(mdcnfg, info->dmc_base + MDCNFG_OFF);
++
++		ddr_hcal = __raw_readl(info->dmc_base + DDR_HCAL_OFF);
++		ddr_hcal |= (1 << HCAL_HCEN_OFFSET);
++		__raw_writel(ddr_hcal, info->dmc_base + DDR_HCAL_OFF);
++
++		return 0;
++	} else if (flag == DMEMC_FREQ_LOW) {
++		pll = 3;
++	} else {
++		pll = 2;
++	}
++
++	mdcnfg = __raw_readl(info->dmc_base + MDCNFG_OFF);
++	mdcnfg &= ~(3 << 28);
++	mdcnfg |= (pll << 28);
++	__raw_writel(mdcnfg, info->dmc_base + MDCNFG_OFF);
++	mdcnfg = __raw_readl(info->dmc_base + MDCNFG_OFF);
++
++	ddr_hcal = __raw_readl(info->dmc_base + DDR_HCAL_OFF);
++	ddr_hcal |= (1 << HCAL_HCEN_OFFSET);
++	__raw_writel(ddr_hcal, info->dmc_base + DDR_HCAL_OFF);
++	ddr_hcal = __raw_readl(info->dmc_base + DDR_HCAL_OFF);
++
++	do {
++		/*pr_debug("polling MDCNFG:0x%x\n", MDCNFG);*/
++		mdcnfg = __raw_readl(info->dmc_base + MDCNFG_OFF);
++	} while (((mdcnfg >> 28) & 0x3) != pll);
++
++	return 0;
++}
++
++static int set_dmc60(void *driver_data, int flag)
++{
++	struct pxa3xx_dvfm_info *info = driver_data;
++	uint32_t accr, reg;
++
++	accr = __raw_readl(info->clkmgr_base + ACCR_OFF);
++	if (flag)
++		accr |= 0x80;
++	else
++		accr &= ~0x80;
++	__raw_writel(accr, info->clkmgr_base + ACCR_OFF);
++	/* polling ACCR */
++	do {
++		reg = __raw_readl(info->clkmgr_base + ACCR_OFF);
++	} while ((accr & 0x80) != (reg & 0x80));
++
++	return 0;
++}
++
++/* set DF and EMPI divider */
++/* TODO: why did not we see DF/EMPI clock as input here? If we want to set DFI_clock or
++ * EMPI clock as other frequecy than 52, how can we do?
++ */
++static int set_df(void *driver_data, int smc)
++{
++	struct pxa3xx_dvfm_info *info = driver_data;
++	uint32_t memclkcfg;
++	int fix_empi;
++
++	if (((info->cpuid > 0x6880) && (info->cpuid <= 0x6881))
++		|| ((info->cpuid >= 0x6890) && (info->cpuid <= 0x6892)))
++		/* It's PXA300 or PXA310 */
++		fix_empi = 1;
++	else
++		fix_empi = 0;
++
++	memclkcfg = __raw_readl(info->smc_base + MEMCLKCFG_OFF);
++	memclkcfg &= ~((7 << MEMCLKCFG_DF_OFFSET) | (7 << MEMCLKCFG_EMPI_OFFSET));
++	if (fix_empi) {
++		memclkcfg |= (3 << MEMCLKCFG_EMPI_OFFSET);
++		switch (smc) {
++		case 208:
++			/* divider -- 4 */
++			memclkcfg |= (3 << MEMCLKCFG_DF_OFFSET);
++			break;
++		case 104:
++			/* divider -- 2 */
++			memclkcfg |= (2 << MEMCLKCFG_DF_OFFSET);
++			break;
++		case 78:
++			/* divider -- 4 */
++			memclkcfg |= (3 << MEMCLKCFG_DF_OFFSET);
++			break;
++		}
++	} else {
++		switch (smc) {
++		case 208:
++			/* divider -- 4 */
++			memclkcfg |= (3 << MEMCLKCFG_DF_OFFSET);
++			memclkcfg |= (3 << MEMCLKCFG_EMPI_OFFSET);
++			break;
++		case 104:
++			/* divider -- 2 */
++			memclkcfg |= (2 << MEMCLKCFG_DF_OFFSET);
++			memclkcfg |= (2 << MEMCLKCFG_EMPI_OFFSET);
++			break;
++		case 78:
++			/* divider -- 4 */
++			memclkcfg |= (3 << MEMCLKCFG_DF_OFFSET);
++			memclkcfg |= (3 << MEMCLKCFG_EMPI_OFFSET);
++			break;
++		}
++	}
++	__raw_writel(memclkcfg, info->smc_base + MEMCLKCFG_OFF);
++	memclkcfg = __raw_readl(info->smc_base + MEMCLKCFG_OFF);
++
++	return 0;
++}
++
++/* TODO: sugguest to differentiate the operating point definition from
++ * register info.And we can remove *reg_new here, and convert dvfm_md_opt to
++ * it in the routine. That will make it much more clear.
++ */
++static int update_hss(void *driver_data, struct dvfm_md_opt *old, struct dvfm_md_opt *new,
++			struct pxa3xx_fv_info *fv_info)
++{
++	struct pxa3xx_dvfm_info *info = driver_data;
++	unsigned int accr, acsr;
++
++	if (old->hss != new->hss) {
++		accr = __raw_readl(info->clkmgr_base + ACCR_OFF);
++		accr &= ~ACCR_HSS_MASK;
++		accr |= (fv_info->hss << ACCR_HSS_OFFSET);
++		__raw_writel(accr, info->clkmgr_base + ACCR_OFF);
++		/* wait until ACSR is changed */
++		do {
++			accr = __raw_readl(info->clkmgr_base + ACCR_OFF) ;
++			acsr = __raw_readl(info->clkmgr_base + ACSR_OFF) ;
++		}while ((accr & ACCR_HSS_MASK) != (acsr & ACCR_HSS_MASK));
++		/* clk_disable(info->lcd_clk);*/
++		/* set PCD just after HSS updated */
++		pxafb_set_pcd();
++		/* clk_enable(info->lcd_clk);*/
++	}
++
++	return 0;
++}
++
++static int update_bus_freq(void *driver_data, struct dvfm_md_opt *old, struct dvfm_md_opt *new)
++{
++	struct pxa3xx_dvfm_info *info = driver_data;
++	struct pxa3xx_fv_info fv_info;
++	uint32_t accr, acsr, mdcnfg, mask;
++	int timeout, dmcflag = 1;
++
++	freq2reg(&fv_info, new);
++	if (old->dmcfs < new->dmcfs)
++		prepare_dmc(info, DMEMC_FREQ_HIGH);
++	else if (old->dmcfs > new->dmcfs)
++		prepare_dmc(info, DMEMC_FREQ_LOW);
++	else
++		dmcflag = 0;
++	if (new->smcfs == 208 || new->smcfs == 78)
++		set_df(info, new->smcfs);
++
++	accr = __raw_readl(info->clkmgr_base + ACCR_OFF);
++	mask = 0;
++	if (old->smcfs != new->smcfs) {
++		accr &= ~ACCR_SMCFS_MASK;
++		accr |= (fv_info.smcfs << ACCR_SMCFS_OFFSET);
++		mask |= ACCR_SMCFS_MASK;
++	}
++	if (old->sflfs != new->sflfs) {
++		accr &= ~ACCR_SFLFS_MASK;
++		accr |= (fv_info.sflfs << ACCR_SFLFS_OFFSET);
++		mask |= ACCR_SFLFS_MASK;
++	}
++	if (old->dmcfs != new->dmcfs) {
++		accr &= ~ACCR_DMCFS_MASK;
++		accr |= (fv_info.dmcfs << ACCR_DMCFS_OFFSET);
++		mask |= ACCR_DMCFS_MASK;
++	}
++	__raw_writel(accr, info->clkmgr_base + ACCR_OFF);
++	accr = __raw_readl(info->clkmgr_base + ACCR_OFF);
++
++	/* wait until ACSR is changed */
++	do {
++		accr = __raw_readl(info->clkmgr_base + ACCR_OFF);
++		acsr = __raw_readl(info->clkmgr_base + ACSR_OFF);
++	} while ((accr & mask) != (acsr & mask));
++
++	if (dmcflag) {
++		timeout = 10;
++		do {
++			mdcnfg = __raw_readl(info->dmc_base + MDCNFG_OFF);
++			udelay(1);
++			if (--timeout == 0) {
++				printk(KERN_WARNING "MDCNFG[29:28] isn't zero\n");
++				break;
++			}
++		} while (mdcnfg & ( 3 << 28));
++	}
++
++	if (new->smcfs == 104) {
++		set_df(info, new->smcfs);
++	}
++
++	update_hss(info, old, new, &fv_info);
++
++	return 0;
++}
++
++static int set_freq(void *driver_data, struct dvfm_md_opt *old, struct dvfm_md_opt *new)
++{
++	struct pxa3xx_dvfm_info *info = driver_data;
++	int spll;
++	uint32_t accr, acsr;
++
++	/* check whether new OP is single PLL mode */
++	if ((new->xl == 0x0c) && (new->xn == 0x1))
++		spll = 1;
++	else
++		spll = 0;
++
++	/* turn on Grayback PLL */
++	if (!spll & !check_grayback_pll(info))
++		set_grayback_pll(info ,1);
++	if (check_mts(old, new) == 2)
++		set_mts(info, 2);
++
++	accr = __raw_readl(info->clkmgr_base + ACCR_OFF);
++	accr &= ~(ACCR_XL_MASK | ACCR_XN_MASK | ACCR_XSPCLK_MASK);
++	accr |= ((new->xl << ACCR_XL_OFFSET) | (new->xn << ACCR_XN_OFFSET)
++			| (3 << ACCR_XSPCLK_OFFSET));
++	__raw_writel(accr, info->clkmgr_base + ACCR_OFF);
++	/* delay 2 cycles of 13MHz clock */
++	udelay(1);
++
++	if (check_mts(old, new) == 1)
++		set_mts(info, 1);
++
++	if ((new->xl == old->xl) && (new->xn != old->xn))
++		/* set T bit */
++		pxa_clkcfg_write(1);
++	else
++		/* set F bit */
++		pxa_clkcfg_write(2);
++	do {
++		accr = __raw_readl(info->clkmgr_base + ACCR_OFF);
++		acsr = __raw_readl(info->clkmgr_base + ACSR_OFF);
++	} while ((accr & (ACCR_XL_MASK | ACCR_XN_MASK))
++			!= (acsr & (ACCR_XL_MASK | ACCR_XN_MASK)));
++
++	udelay(1);
++	update_bus_freq(info, old, new);
++
++	/* turn off Grayback PLL */
++	if (spll)
++		set_grayback_pll(info, 0);
++	return 0;
++}
++
++static int update_freq(void *driver_data, struct dvfm_freqs *freqs)
++{
++	struct pxa3xx_dvfm_info *info = driver_data;
++	static struct dvfm_md_opt before_d0cs;
++	struct dvfm_md_opt old, new;
++	struct op_info *p = NULL;
++	unsigned long flags;
++	int found = 0, new_op = cur_op;
++
++	memset(&old, 0, sizeof(struct dvfm_md_opt));
++	memset(&new, 0, sizeof(struct dvfm_md_opt));
++	write_lock_irqsave(&pxa3xx_dvfm_op_list.lock, flags);
++	if (!list_empty(&pxa3xx_dvfm_op_list.list)) {
++		list_for_each_entry(p, &pxa3xx_dvfm_op_list.list, list) {
++			if (p->index == freqs->old) {
++				found++;
++				memcpy(&old, (struct dvfm_md_opt *)p->op,
++					sizeof(struct dvfm_md_opt));
++			}
++			if (p->index == freqs->new) {
++				found++;
++				memcpy(&new, (struct dvfm_md_opt *)p->op,
++					sizeof(struct dvfm_md_opt));
++				new_op = p->index;
++			}
++			if (found == 2)
++				break;
++		}
++	}
++	write_unlock_irqrestore(&pxa3xx_dvfm_op_list.lock, flags);
++	if (found != 2)
++		return -EINVAL;
++
++	if ((old.power_mode == POWER_MODE_D0)
++		&& (new.power_mode == POWER_MODE_D0CS)) {
++		memcpy(&before_d0cs, &old, sizeof(struct dvfm_md_opt));
++
++		pxa3xx_enter_d0cs(info);
++		update_voltage(info, &old, &new);
++		cur_op = new_op;
++		loops_per_jiffy = new.lpj;
++		return 0;
++	} else if ((old.power_mode == POWER_MODE_D0CS)
++		&& (new.power_mode == POWER_MODE_D0)) {
++		if (memcmp(&before_d0cs, &new, sizeof(struct dvfm_md_opt))) {
++			/* exit d0cs and set new operating point */
++			if ((before_d0cs.vcc_core < new.vcc_core) ||
++				(before_d0cs.vcc_sram < new.vcc_sram)) {
++				update_voltage(info, &old, &new);
++			} else {
++				update_voltage(info, &old, &before_d0cs);
++			}
++			pxa3xx_exit_d0cs(info);
++			set_freq(info, &before_d0cs, &new);
++
++			if ((before_d0cs.vcc_core > new.vcc_core) ||
++				(before_d0cs.vcc_sram > new.vcc_sram))
++				update_voltage(info, &before_d0cs, &new);
++		} else {
++			update_voltage(info, &old, &new);
++			/* exit d0cs */
++			pxa3xx_exit_d0cs(info);
++		}
++		cur_op = new_op;
++		loops_per_jiffy = new.lpj;
++		return 0;
++	} else if ((old.power_mode == POWER_MODE_D0CS)
++		&& (new.power_mode == POWER_MODE_D0CS)) {
++		cur_op = new_op;
++		return 0;
++	}
++
++	if (old.core < new.core) {
++		update_voltage(info, &old, &new);
++	}
++	set_freq(info, &old, &new);
++	if (old.core > new.core) {
++		update_voltage(info, &old, &new);
++	}
++	cur_op = new_op;
++	if ((new.power_mode == POWER_MODE_D0)
++		|| (new.power_mode == POWER_MODE_D0CS))
++		loops_per_jiffy = new.lpj;
++	return 0;
++}
++
++/* function of entering low power mode */
++extern void enter_lowpower_mode(int state);
++
++static void do_freq_notify(void *driver_data, struct dvfm_freqs *freqs)
++{
++	struct pxa3xx_dvfm_info *info = driver_data;
++
++	dvfm_notifier_frequency(freqs, DVFM_FREQ_PRECHANGE);
++	update_freq(info, freqs);
++	dvfm_notifier_frequency(freqs, DVFM_FREQ_POSTCHANGE);
++	ispt_dvfm_op(freqs->old, freqs->new);
++
++	printk("-- dvfm: cur_op=%d\n",cur_op);
++}
++
++static void do_lowpower_notify(void *driver_data, struct dvfm_freqs *freqs, unsigned int state)
++{
++	dvfm_notifier_frequency(freqs, DVFM_FREQ_PRECHANGE);
++	//enter_lowpower_mode(state);
++	dvfm_notifier_frequency(freqs, DVFM_FREQ_POSTCHANGE);
++	ispt_power_state_d2();
++}
++
++static int check_op(void *driver_data, struct dvfm_freqs *freqs, unsigned int new,
++			unsigned int relation)
++{
++	struct op_info *p = NULL;
++	struct dvfm_md_opt *q = NULL;
++	int core, tmp_core = -1, found = 0;
++	int first_op = 0;
++
++	freqs->new = -1;
++	if (!dvfm_find_op(new, &p)) {
++		q = (struct dvfm_md_opt *)p->op;
++		core = q->core;
++	} else
++		return -EINVAL;
++	/*
++	pr_debug("%s, old:%d, new:%d, core:%d\n", __FUNCTION__, freqs->old,
++		new, core);
++	*/
++	read_lock(&pxa3xx_dvfm_op_list.lock);
++	if (relation == RELATION_LOW) {
++		/* Set the lowest frequency that is higher than specifed one */
++		list_for_each_entry(p, &pxa3xx_dvfm_op_list.list, list) {
++			q = (struct dvfm_md_opt *)p->op;
++			if (core == 0) {
++				/* Lowpower mode */
++				if ((q->power_mode == POWER_MODE_D1)
++					|| (q->power_mode == POWER_MODE_D2)
++					|| (q->power_mode == POWER_MODE_CG)) {
++					if (!p->device && (new == p->index)) {
++						freqs->new = p->index;
++						/*
++						pr_debug("%s, found op%d\n",
++							__FUNCTION__, p->index);
++						*/
++						break;
++					}
++				}
++				continue;
++			}
++
++			if (!p->device && (q->core >= core)) {
++				if (tmp_core == -1 || (tmp_core >= q->core)) {
++					/*
++					pr_debug("%s, found op%d, core:%d\n",
++						__FUNCTION__, p->index,
++						q->core);
++					*/
++					if (first_op == 0)
++						first_op = p->index;
++					freqs->new = p->index;
++					tmp_core = q->core;
++					found = 1;
++				}
++				if (found && (new == p->index))
++					break;
++			}
++		}
++		if (found && (first_op == 1) && (new != p->index))
++			freqs->new = first_op;
++	} else if (relation == RELATION_HIGH) {
++		/* Set the highest frequency that is lower than specified one */
++		list_for_each_entry(p, &pxa3xx_dvfm_op_list.list, list) {
++			q = (struct dvfm_md_opt *)p->op;
++			if (!p->device && (q->core <= core)) {
++				if (tmp_core == -1 || tmp_core < q->core) {
++					freqs->new = p->index;
++					tmp_core = q->core;
++				}
++			}
++		}
++	} else if (relation == RELATION_STICK) {
++		/* Set the specified frequency */
++		list_for_each_entry(p, &pxa3xx_dvfm_op_list.list, list) {
++			if (!p->device && (p->index == new)) {
++				freqs->new = p->index;
++				break;
++			}
++		}
++	}
++	read_unlock(&pxa3xx_dvfm_op_list.lock);
++	if (freqs->new == -1) {
++		/*
++		pr_debug("%s, Can't find op\n", __FUNCTION__);
++		pr_debug("%s, old:%d, new:%d, core:%d\n", __FUNCTION__,
++			freqs->old, new, core);
++		*/
++		return -EINVAL;
++	}
++	return 0;
++}
++
++static int pxa3xx_get_freq(void *driver_data, struct op_info *p, struct op_freq *freq)
++{
++        struct dvfm_md_opt *q = (struct dvfm_md_opt *)p->op;
++	freq->cpu_freq = q->core;
++	return 0;
++}
++
++static int pxa3xx_check_active_op(void *driver_data, struct op_info *p)
++{
++	struct dvfm_md_opt *q = (struct dvfm_md_opt *)p->op;
++
++	if ((!strcmp(q->name, "D0CS")) && (boot_core_freq >= q->core))
++		return 0;
++
++        if ((!strcmp(q->name, "104M")) && (boot_core_freq >= q->core))
++                return 0;
++
++	if ((!strcmp(q->name, "156M")) && (boot_core_freq >= q->core))
++		return 0;
++
++	if ((!strcmp(q->name, "208M")) && (boot_core_freq >= q->core))
++		return 0;
++
++	if ((!strcmp(q->name, "416M")) && (boot_core_freq >= q->core))
++		return 0;
++
++	if ((!strcmp(q->name, "624M")) && (boot_core_freq >= q->core))
++		return 0;
++
++	return -EINVAL;
++}
++
++
++static int pxa3xx_set_op(void *driver_data, struct dvfm_freqs *freqs, unsigned int new,
++			unsigned int relation)
++{
++	struct pxa3xx_dvfm_info *info = driver_data;
++	struct dvfm_md_opt *md = NULL, *old_md = NULL;
++	struct op_info *p = NULL;
++	unsigned long flags;
++	int ret;
++	out_d0cs = 0;
++
++	local_fiq_disable();
++	local_irq_save(flags);
++	ret = dvfm_find_op(freqs->old, &p);
++	if (ret) {
++		printk("---- pxa3xx_set_op1 check_op failed to %d\n",new);
++		goto out;
++	}
++
++	memcpy(&freqs->old_info, p, sizeof(struct op_info));
++	ret = check_op(info, freqs, new, relation);
++	if (ret) {
++		printk("---- pxa3xx_set_op2 check_op failed to %d\n",new);
++		goto out;
++	}
++
++	if (!dvfm_find_op(freqs->new, &p)) {
++		memcpy(&(freqs->new_info), p, sizeof(struct op_info));
++		/* If find old op and new op is same, skip it.
++		 * At here, ret should be zero.
++		 */
++		if (freqs->old_info.index == freqs->new_info.index)
++			goto out;
++#ifdef DVFM_LP_SAFE
++		md = (struct dvfm_md_opt *)(freqs->new_info.op);
++		old_md = (struct dvfm_md_opt *)(freqs->old_info.op);
++		if ((old_md->power_mode == POWER_MODE_D0CS)
++			&& ((md->power_mode == POWER_MODE_D1)
++			|| (md->power_mode == POWER_MODE_D2))) {
++			dvfm_disable_op_name("D0CS", dvfm_dev_id);
++			out_d0cs = 1;
++		}
++
++		md = (struct dvfm_md_opt *)p->op;
++		switch (md->power_mode) {
++		case POWER_MODE_D0:
++		case POWER_MODE_D0CS:
++			do_freq_notify(info, freqs);
++			break;
++		case POWER_MODE_D1:
++		case POWER_MODE_D2:
++		case POWER_MODE_CG:
++			do_lowpower_notify(info, freqs, md->power_mode);
++			break;
++		}
++		local_irq_restore(flags);
++		local_fiq_enable();
++
++		if (out_d0cs) {
++			dvfm_enable_op_name("D0CS", dvfm_dev_id);
++		}
++#else
++		md = (struct dvfm_md_opt *)p->op;
++		switch (md->power_mode) {
++		case POWER_MODE_D0:
++		case POWER_MODE_D0CS:
++			do_freq_notify(info, freqs);
++			break;
++		case POWER_MODE_D1:
++		case POWER_MODE_D2:
++		case POWER_MODE_CG:
++			do_lowpower_notify(info, freqs, md->power_mode);
++			break;
++		}
++		local_irq_restore(flags);
++		local_fiq_enable();
++#endif
++	}
++	return 0;
++out:
++	local_irq_restore(flags);
++	local_fiq_enable();
++	return ret;
++}
++
++static int pxa3xx_request_op(void *driver_data, int index)
++{
++	struct dvfm_freqs freqs;
++	struct op_info *info = NULL;
++	struct dvfm_md_opt *md = NULL;
++	int relation, ret;
++	ret = dvfm_find_op(index, &info);
++	if (ret)
++		goto out;
++	freqs.old = cur_op;
++	freqs.new = index;
++	md = (struct dvfm_md_opt *)(info->op);
++	switch (md->power_mode) {
++	case POWER_MODE_D1:
++	case POWER_MODE_D2:
++	case POWER_MODE_CG:
++		relation = RELATION_STICK;
++		ret = pxa3xx_set_op(driver_data, &freqs, index, relation);
++		break;
++	default:
++		relation = RELATION_LOW;
++		/* only use non-low power mode as preferred op */
++		ret = pxa3xx_set_op(driver_data, &freqs, index, relation);
++		if (!ret)
++			preferred_op = index;
++		break;
++	}
++out:
++	return ret;
++}
++
++static int is_d0cs(void *driver_data)
++{
++	struct pxa3xx_dvfm_info *info = driver_data;
++	unsigned int acsr;
++	/* read ACSR */
++	acsr = __raw_readl(info->clkmgr_base + ACSR_OFF);
++	/* Check ring oscillator status */
++	if (acsr & (1 << 26))
++		return 1;
++	return 0;
++}
++
++/* Produce a operating point table */
++static int op_init(void *driver_data, struct info_head *op_table)
++{
++	struct pxa3xx_dvfm_info *info = driver_data;
++	unsigned long flags;
++	int i, index;
++	struct op_info *p = NULL, *q = NULL;
++	struct dvfm_md_opt *md = NULL, *smd = NULL;
++	struct proc_op_array *proc = NULL;
++
++	write_lock_irqsave(&op_table->lock, flags);
++	for (i = 0; i < ARRAY_SIZE(proc_op_arrays); i++){
++		if (proc_op_arrays[i].cpuid == (info->cpuid & 0xfff0)) {
++			proc = &proc_op_arrays[i];
++			break;
++		}
++	}
++	if (proc == NULL) {
++		printk(KERN_ERR "Failed to find op tables for cpu_id 0x%08x", info->cpuid);
++		write_unlock_irqrestore(&op_table->lock, flags);
++		return -EIO;
++	} else {
++		printk("initializing op table for %s\n", proc->cpu_name);
++	}
++	for (i = 0, index = 0; i < proc->nr_op; i++) {
++		/* PXA310 A2 or PXA935/PXA940, dmcfs 60MHz in S0D0CS mode */
++		if ((proc->op_array[i].power_mode == POWER_MODE_D0CS)
++			&& (info->cpuid == 0x6892 || (info->cpuid & 0xFFF0) == 0x6930)) {
++			set_dmc60(info, 1);
++			proc->op_array[i].dmcfs = 60;
++		}
++
++		/* Set index of operating point used in idle */
++		if (proc->op_array[i].power_mode != POWER_MODE_D0) {
++			//set_idle_op(index, proc->op_array[i].power_mode);
++		}
++
++		md = (struct dvfm_md_opt *)kzalloc(sizeof(struct dvfm_md_opt),
++					GFP_KERNEL);
++		p = (struct op_info *)kzalloc(sizeof(struct op_info),
++					GFP_KERNEL);
++		p->op = (void *)md;
++		memcpy(p->op, &proc->op_array[i], sizeof(struct dvfm_md_opt));
++		md->core = 13 * md->xl * md->xn;
++		if (md->power_mode == POWER_MODE_D0CS)
++			md->core = 60;
++		p->index = index++;
++		list_add_tail(&(p->list), &(op_table->list));
++	}
++	md = (struct dvfm_md_opt *)kzalloc(sizeof(struct dvfm_md_opt),
++					GFP_KERNEL);
++	p = (struct op_info *)kzalloc(sizeof(struct op_info), GFP_KERNEL);
++	p->op = (void *)md;
++	if (capture_op_info(info, md)) {
++		printk(KERN_WARNING "Failed to get current op setting\n");
++	} else {
++		def_op = 0x5a5a;	/* magic number */
++		list_for_each_entry(q, &(op_table->list), list) {
++			smd = (struct dvfm_md_opt *)q->op;
++			md->flag = smd->flag;
++			md->lpj = smd->lpj;
++			md->core = smd->core;
++			if (memcmp(md, smd, sizeof(struct dvfm_md_opt)) == 0) {
++				def_op = q->index;
++				break;
++			}
++		}
++	}
++	if (is_d0cs(driver_data))
++		md->core = 60;
++	else
++		md->core = 13 * md->xl * md->xn;
++	md->lpj = loops_per_jiffy;
++	md->flag = OP_FLAG_BOOT;
++	sprintf(md->name, "BOOT OP");
++
++	boot_core_freq = md->core;
++
++#if 0   /* disable CUSTOM OP for borq platfrom */
++	smd = (struct dvfm_md_opt *)kzalloc(sizeof(struct dvfm_md_opt),
++					GFP_KERNEL);
++	q = (struct op_info *)kzalloc(sizeof(struct op_info), GFP_KERNEL);
++	memcpy(q, p, sizeof(struct op_info));
++	memcpy(smd, md, sizeof(struct dvfm_md_opt));
++	smd->core = md->core;
++	smd->lpj = md->lpj;
++	smd->flag = OP_FLAG_USER_DEFINED;
++	sprintf(smd->name, "CUSTOM OP");
++	q->op = (void *)smd;
++	/* Add CUSTOM OP into op list */
++	q->index = index++;
++	list_add_tail(&q->list, &op_table->list);
++#endif
++	/* Add BOOT OP into op list */
++	p->index = index++;
++	preferred_op = p->index;
++	list_add_tail(&p->list, &op_table->list);
++	/* BOOT op */
++	if (def_op == 0x5a5a) {
++		cur_op = p->index;
++		def_op = p->index;
++	} else
++		cur_op = def_op;
++	pr_debug("%s, def_op:%d, cur_op:%d\n", __FUNCTION__, def_op, cur_op);
++
++	op_nums = proc->nr_op + 2;	/* set the operating point number */
++
++	pr_debug("Current Operating Point is %d\n", cur_op);
++	dump_op_list(info, op_table, OP_FLAG_ALL);
++	write_unlock_irqrestore(&op_table->lock, flags);
++
++	return 0;
++}
++
++/*
++ * The machine operation of dvfm_enable
++ */
++static int pxa3xx_enable_dvfm(void *driver_data, int dev_id)
++{
++	struct pxa3xx_dvfm_info *info = driver_data;
++	struct dvfm_md_opt *md = NULL;
++	struct op_info *p = NULL;
++	int i, num;
++	num = get_op_num(info, &pxa3xx_dvfm_op_list);
++	for (i = 0; i < num; i++) {
++		if (!dvfm_find_op(i, &p)) {
++			md = (struct dvfm_md_opt *)p->op;
++			if (md->core < boot_core_freq)
++				dvfm_enable_op_name(md->name, dev_id);
++		}
++	}
++	ispt_block_dvfm(0, dev_id);
++	return 0;
++}
++
++/*
++ * The mach operation of dvfm_disable
++ */
++static int pxa3xx_disable_dvfm(void *driver_data, int dev_id)
++{
++	struct pxa3xx_dvfm_info *info = driver_data;
++	struct dvfm_md_opt *md = NULL;
++	struct op_info *p = NULL;
++	int i, num;
++	num = get_op_num(info, &pxa3xx_dvfm_op_list);
++	for (i = 0; i < num; i++) {
++		if (!dvfm_find_op(i, &p)) {
++			md = (struct dvfm_md_opt *)p->op;
++			if (md->core < boot_core_freq)
++				dvfm_disable_op_name(md->name, dev_id);
++		}
++	}
++	ispt_block_dvfm(1, dev_id);
++	return 0;
++}
++
++static int pxa3xx_enable_op(void *driver_data, int index, int relation)
++{
++	/*
++	 * Restore preferred_op. Because this op is sugguested by policy maker
++	 * or user.
++	 */
++	return pxa3xx_request_op(driver_data, preferred_op);
++}
++
++static int pxa3xx_disable_op(void *driver_data, int index, int relation)
++{
++	struct dvfm_freqs freqs;
++	if (cur_op == index) {
++		freqs.old = index;
++		freqs.new = -1;
++		dvfm_set_op(&freqs, freqs.old, relation);
++	}
++	return 0;
++}
++
++static int pxa3xx_volt_show(void *driver_data, char *buf)
++{
++	struct dvfm_md_opt new;
++	int len = 0;
++
++	memset(&new, 0, sizeof(struct dvfm_md_opt));
++	pxa3xx_pmic_get_voltage(VCC_CORE, &new.vcc_core);
++	pxa3xx_pmic_get_voltage(VCC_SRAM, &new.vcc_sram);
++	len = sprintf(buf, "core voltage:%dmv, sram voltage:%dmv\n",
++			new.vcc_core, new.vcc_sram);
++	return len;
++}
++
++#ifdef CONFIG_CPU_PXA310
++static int pxa3xx_freq_show(void *driver_data, struct op_info *p, char *buf)
++{
++	struct dvfm_md_opt *q = (struct dvfm_md_opt *)p->op;
++	struct pxa3xx_fv_info info;
++
++	if (q == NULL)
++		return sprintf(buf, "unable to get frequency info\n");
++	else {
++		freq2reg(&info, q);
++		if (!info.d0cs){
++			return sprintf(buf, "current frequency is %luMhz"
++					" (XL: %lu, XN: %lu, %s) with\n"
++					"  SMEM: %lu (%dMhz)\n"
++					"  SRAM: %lu (%dMhz)\n"
++					"  HSS: %lu (%dMhz)\n"
++					"  DDR: %lu (%dMhz)\n"
++					"  DFCLK: %lu (%dMhz)\n"
++					"  EMPICLK: %lu (%dMhz)\n"
++					"  D0CKEN_A: 0x%08x\n"
++					"  D0CKEN_B: 0x%08x\n"
++					"  ACCR: 0x%08x\n"
++					"  ACSR: 0x%08x\n"
++					"  OSCC: 0x%08x\n",
++					FREQ_CORE(info.xl, info.xn), info.xl, info.xn,
++					(info.xn != 0x1)? "Turbo Mode" : "Run Mode",
++					info.smcfs, FREQ_STMM(info.smcfs),
++					info.sflfs, FREQ_SRAM(info.sflfs),
++					info.hss, FREQ_HSS(info.hss),
++					info.dmcfs, FREQ_DDR(info.dmcfs),
++					info.df_clk, FREQ_DFCLK(info.smcfs, info.df_clk),
++					info.empi_clk, FREQ_EMPICLK(info.smcfs, info.empi_clk),
++					CKENA, CKENB, ACCR, ACSR, OSCC);
++		} else {
++			return sprintf(buf, "current frequency is 60Mhz"
++					" (ring oscillator mode) with\n"
++					"  SMEM:15Mhz\n"
++					"  SRAM:60Mhz\n"
++					"  HSS:60Mhz\n"
++					"  DDR:30Mhz\n"
++					"  DFCLK:%sMhz\n"
++					"  EMPICLK:%sMhz\n"
++					"  D0CKEN_A: 0x%08x\n"
++					"  D0CKEN_B: 0x%08x\n"
++					"  ACCR: 0x%08x\n"
++					"  ACSR: 0x%08x\n"
++					"  OSCC: 0x%08x\n",
++					(info.df_clk == 1)?"15":
++					(info.df_clk == 2)?"7.5":
++					(info.df_clk == 3)?"3.75":"0",
++					(info.empi_clk == 1)?"15":
++					(info.empi_clk == 2)?"7.5":
++					(info.empi_clk == 3)?"3.75":"0",
++					CKENA, CKENB, ACCR, ACSR, OSCC);
++		}
++
++
++	}
++}
++#endif
++
++#ifdef CONFIG_PXA3xx_DVFM_STATS
++/* Convert ticks from 32K timer to microseconds */
++static unsigned int pxa3xx_ticks_to_usec(unsigned int ticks)
++{
++	return (ticks * 5 * 5 * 5 * 5 * 5 * 5) >> 9;
++}
++
++static unsigned int pxa3xx_ticks_to_sec(unsigned int ticks)
++{
++	return (ticks >> 15);
++}
++
++static unsigned int pxa3xx_read_time(void)
++{
++	return OSCR4;
++}
++
++/* It's invoked by PM functions.
++ * PM functions can store the accurate time of entering/exiting low power
++ * mode.
++ */
++int calc_switchtime(unsigned int end, unsigned int start)
++{
++	switch_lowpower_before = end;
++	switch_lowpower_after = start;
++	return 0;
++}
++
++static int pxa3xx_stats_notifier_freq(struct notifier_block *nb,
++				unsigned long val, void *data)
++{
++	struct dvfm_freqs *freqs = (struct dvfm_freqs *)data;
++	struct op_info *info = &(freqs->new_info);
++	struct dvfm_md_opt *md = NULL;
++	unsigned int ticks;
++
++	ticks = pxa3xx_read_time();
++	md = (struct dvfm_md_opt *)(info->op);
++	if (md->power_mode == POWER_MODE_D0 ||
++		md->power_mode == POWER_MODE_D0CS) {
++		switch (val) {
++		case DVFM_FREQ_PRECHANGE:
++			calc_switchtime_start(freqs->old, freqs->new, ticks);
++			break;
++		case DVFM_FREQ_POSTCHANGE:
++			/* Calculate the costed time on switching frequency */
++			calc_switchtime_end(freqs->old, freqs->new, ticks);
++			dvfm_add_event(freqs->old, CPU_STATE_RUN,
++					freqs->new, CPU_STATE_RUN);
++			dvfm_add_timeslot(freqs->old, CPU_STATE_RUN);
++			mspm_add_event(freqs->old, CPU_STATE_RUN);
++			break;
++		}
++	} else if (md->power_mode == POWER_MODE_D1 ||
++		md->power_mode == POWER_MODE_D2 ||
++		md->power_mode == POWER_MODE_CG) {
++		switch (val) {
++		case DVFM_FREQ_PRECHANGE:
++			calc_switchtime_start(freqs->old, freqs->new, ticks);
++			/* Consider lowpower mode as idle mode */
++			dvfm_add_event(freqs->old, CPU_STATE_RUN,
++					freqs->new, CPU_STATE_IDLE);
++			dvfm_add_timeslot(freqs->old, CPU_STATE_RUN);
++			mspm_add_event(freqs->old, CPU_STATE_RUN);
++			break;
++		case DVFM_FREQ_POSTCHANGE:
++			/* switch_lowpower_start before switch_lowpower_after
++			 * is updated in calc_switchtime().
++			 * It's invoked in pm function.
++			 */
++			calc_switchtime_end(freqs->old, freqs->new,
++					switch_lowpower_before);
++			calc_switchtime_start(freqs->new, freqs->old,
++					switch_lowpower_after);
++			calc_switchtime_end(freqs->new, freqs->old,
++					ticks);
++			dvfm_add_event(freqs->new, CPU_STATE_IDLE,
++					freqs->old, CPU_STATE_RUN);
++			dvfm_add_timeslot(freqs->new, CPU_STATE_IDLE);
++			mspm_add_event(freqs->new, CPU_STATE_IDLE);
++			break;
++		}
++	}
++	return 0;
++}
++#else
++#define pxa3xx_ticks_to_usec	NULL
++#define pxa3xx_ticks_to_sec	NULL
++#define pxa3xx_read_time	NULL
++#endif
++
++static struct dvfm_driver pxa3xx_driver = {
++	.count	= get_op_num,
++	.set	= pxa3xx_set_op,
++	.dump	= dump_op,
++	.name	= get_op_name,
++	.request_set	= pxa3xx_request_op,
++	.enable_dvfm	= pxa3xx_enable_dvfm,
++	.disable_dvfm	= pxa3xx_disable_dvfm,
++	.enable_op	= pxa3xx_enable_op,
++	.disable_op	= pxa3xx_disable_op,
++	.volt_show	= pxa3xx_volt_show,
++#ifdef CONFIG_CPU_PXA310
++	.freq_show	= pxa3xx_freq_show,
++#endif
++	.ticks_to_usec	= pxa3xx_ticks_to_usec,
++	.ticks_to_sec	= pxa3xx_ticks_to_sec,
++	.read_time	= pxa3xx_read_time,
++	.get_freq	= pxa3xx_get_freq,
++	.check_active_op = pxa3xx_check_active_op,
++};
++
++#ifdef CONFIG_PM
++static int pxa3xx_freq_suspend(struct platform_device *pdev, pm_message_t state)
++{
++	current_op = cur_op;
++	dvfm_request_op(1);
++	return 0;
++}
++
++static int pxa3xx_freq_resume(struct platform_device *pdev)
++{
++	dvfm_request_op(current_op);
++	return 0;
++}
++#else
++#define pxa3xx_freq_suspend    NULL
++#define pxa3xx_freq_resume     NULL
++#endif
++
++static void pxa3xx_poweri2c_init(struct pxa3xx_dvfm_info *info)
++{
++	uint32_t avcr, svcr, cvcr, pcfr, pvcr;
++
++	if ((info->flags & PXA3xx_USE_POWER_I2C) &&
++		((info->cpuid & 0xfff0) == 0x6930)) {
++		/* set AVCR for PXA935/PXA940:
++		 *	level 0: 1250mv, 0x15
++		 *	level 1: 1250mv, 0x15
++		 *	level 2: 1250mv, 0x15
++		 *	level 3: 1250mv, 0x15
++		 */
++		avcr = __raw_readl(info->spmu_base + AVCR_OFF);
++		avcr &= 0xE0E0E0E0;
++		avcr |= (0x15 << 24) | (0x15 << 16) | (0x15 << 8) | 0x15;
++		__raw_writel(avcr, info->spmu_base + AVCR_OFF);
++		avcr = __raw_readl(info->spmu_base + AVCR_OFF);
++
++		/* set delay */
++		pcfr = __raw_readl(info->spmu_base + PCFR_OFF);
++		pcfr &= 0x000FFFFF;
++		pcfr |= 0xCCF00000;
++		/* Disable pullup/pulldown in PWR_SCL and PWR_SDA */
++		pcfr |= 0x04;
++		__raw_writel(pcfr, info->spmu_base + PCFR_OFF);
++		pcfr = __raw_readl(info->spmu_base + PCFR_OFF);
++
++		/* enable FVE,PVE,TVE bit */
++		__raw_writel(0xe0500034, info->spmu_base + PVCR_OFF);
++	} else if (info->flags & PXA3xx_USE_POWER_I2C) {
++		/* set AVCR for PXA300/PXA310/PXA320/PXA930
++		 *	level 0: 1000mv, 0x0b
++		 *	level 1: 1100mv, 0x0f
++		 *	level 2: 1375mv, 0x1a
++		 *	level 3: 1400mv, 0x1b
++		 */
++		avcr = __raw_readl(info->spmu_base + AVCR_OFF);
++		avcr &= 0xE0E0E0E0;
++		/* PXA930 B0(cpuid 0x6835) requires special setting */
++		if (info->cpuid == 0x6835)
++			avcr |= (0x1b << 24) | (0x1a << 16) | (0x0f << 8) | 0xb;
++		else
++			avcr |= (0x0f << 24) | (0x1a << 16) | (0x0f << 8) | 0xb;
++		__raw_writel(avcr, info->spmu_base + AVCR_OFF);
++		avcr = __raw_readl(info->spmu_base + AVCR_OFF);
++		/* set SVCR:
++		 * 	level 0: 1100mv, 0x0f
++		 * 	level 1: 1200mv, 0x13
++		 * 	level 2: 1400mv, 0x1b
++		 * 	level 3: 1400mv, 0x1b
++		 */
++		svcr = __raw_readl(info->spmu_base + SVCR_OFF);
++		svcr &= 0xE0E0E0E0;
++		if (info->cpuid == 0x6835)
++			svcr |= (0x1b << 24) | (0x1b << 16) | (0x13 << 8) | 0xf;
++		else
++			svcr |= (0x0f << 24) | (0x1b << 16) | (0x13 << 8) | 0xf;
++		__raw_writel(svcr, info->spmu_base + SVCR_OFF);
++		svcr = __raw_readl(info->spmu_base + SVCR_OFF);
++		/* set CVCR:
++		 * 	level 0: 925mv, 0x08
++		 * 	level 1: 1250mv, 0x15
++		 * 	level 2: 1375mv, 0x1a
++		 * 	level 3: 1400mv, 0x1b
++		 */
++		cvcr = __raw_readl(info->spmu_base + CVCR_OFF);
++		cvcr &= 0xE0E0E0E0;
++		if (info->cpuid == 0x6835)
++			cvcr |= (0x1b << 24) | (0x1a << 16) | (0x15 << 8) | 0x08;
++		else
++			cvcr |= (0x0f << 24) | (0x1a << 16) | (0x15 << 8) | 0x08;
++		__raw_writel(cvcr, info->spmu_base + CVCR_OFF);
++		cvcr = __raw_readl(info->spmu_base + CVCR_OFF);
++
++		/* set delay */
++		pcfr = __raw_readl(info->spmu_base + PCFR_OFF);
++		pcfr &= 0x000FFFFF;
++		pcfr |= 0xCCF00000;
++		/* Disable pullup/pulldown in PWR_SCL and PWR_SDA */
++		pcfr |= 0x04;
++		__raw_writel(pcfr, info->spmu_base + PCFR_OFF);
++		pcfr = __raw_readl(info->spmu_base + PCFR_OFF);
++
++		/* enable FVE,PVE,TVE bit */
++		__raw_writel(0xe0500034, info->spmu_base + PVCR_OFF);
++	} else {
++		/* disable FVE,PVE,TVE,FVC bit */
++		pvcr = __raw_readl(info->spmu_base + PVCR_OFF);
++		pvcr &= 0x0fffffff;
++		__raw_writel(pvcr, info->spmu_base + PVCR_OFF);
++	}
++}
++
++int gpio_reset_work_around(void)
++{
++	dvfm_disable_op_name("624M", dvfm_dev_id);
++	dvfm_disable_op_name("416M", dvfm_dev_id);
++	dvfm_disable_op_name("208M", dvfm_dev_id);
++	return 0;
++}
++
++static int pxa3xx_freq_probe(struct platform_device *pdev)
++{
++	struct resource *res;
++	struct pxa3xx_freq_mach_info *pdata;
++	struct pxa3xx_dvfm_info *info;
++	int rc;
++
++	/* initialize the information necessary to frequency/voltage change operation */
++	pdata = pdev->dev.platform_data;
++	info = kzalloc(sizeof(struct pxa3xx_dvfm_info), GFP_KERNEL);
++	info->flags = pdata->flags;
++	info->cpuid = read_cpuid(0) & 0xFFFF;
++
++	//info->lcd_clk = clk_get(&pxa_device_fb.dev, "LCDCLK");
++	//if (IS_ERR(info->lcd_clk)) goto err;
++
++	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "clkmgr_regs");
++	if (!res) goto err;
++	info->clkmgr_base = ioremap(res->start, res->end - res->start + 1);
++
++	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "spmu_regs");
++	if (!res) goto err;
++	info->spmu_base = ioremap(res->start, res->end - res->start + 1);
++
++	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "bpmu_regs");
++	if (!res) goto err;
++	info->bpmu_base = ioremap(res->start, res->end - res->start + 1);
++
++	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "dmc_regs");
++	if (!res) goto err;
++	info->dmc_base = ioremap(res->start, res->end - res->start + 1);
++
++	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "smc_regs");
++	if (!res) goto err;
++	info->smc_base = ioremap(res->start, res->end - res->start + 1);
++
++	pxa3xx_driver.priv = info;
++
++	pxa3xx_poweri2c_init(info);
++	op_init(info, &pxa3xx_dvfm_op_list);
++
++	return dvfm_register_driver(&pxa3xx_driver, &pxa3xx_dvfm_op_list);
++err:
++	printk("pxa3xx_dvfm init failed\n");
++	return -EIO;
++}
++
++static int pxa3xx_freq_remove(struct platform_device *pdev)
++{
++	kfree(pxa3xx_driver.priv);
++	return dvfm_unregister_driver(&pxa3xx_driver);
++}
++
++static struct platform_driver pxa3xx_freq_driver = {
++	.driver = {
++		.name	= "pxa3xx-freq",
++	},
++	.probe		= pxa3xx_freq_probe,
++	.remove		= pxa3xx_freq_remove,
++#ifdef CONFIG_PM
++	//.suspend	= pxa3xx_freq_suspend,
++	//.resume		= pxa3xx_freq_resume,
++#endif
++};
++
++
++static int __init pxa3xx_freq_init(void)
++{
++	int ret;
++	ret = platform_driver_register(&pxa3xx_freq_driver);
++	if (ret)
++		goto out;
++#ifdef CONFIG_PXA3xx_DVFM_STATS
++	ret = dvfm_register_notifier(&notifier_freq_block,
++				DVFM_FREQUENCY_NOTIFIER);
++#endif
++	ret = dvfm_register("DVFM", &dvfm_dev_id);
++out:
++	return ret;
++}
++
++static void __exit pxa3xx_freq_exit(void)
++{
++#ifdef CONFIG_PXA3xx_DVFM_STATS
++	dvfm_unregister_notifier(&notifier_freq_block,
++				DVFM_FREQUENCY_NOTIFIER);
++#endif
++	dvfm_unregister("DVFM", &dvfm_dev_id);
++	platform_driver_unregister(&pxa3xx_freq_driver);
++}
++
++module_init(pxa3xx_freq_init);
++module_exit(pxa3xx_freq_exit);
++
+diff -ur linux-2.6.32/arch/arm/mach-pxa/pxa3xx_dvfm_ll.S kernel/arch/arm/mach-pxa/pxa3xx_dvfm_ll.S
+--- linux-2.6.32/arch/arm/mach-pxa/pxa3xx_dvfm_ll.S	2009-12-13 13:00:42.108609192 +0200
++++ kernel/arch/arm/mach-pxa/pxa3xx_dvfm_ll.S	2009-12-12 16:09:26.482948915 +0200
+@@ -0,0 +1,261 @@
++@
++@ 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., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
++@
++@
++@  FILENAME:       pxa3xx_dvfm_ll.S
++@
++@  PURPOSE:        Provides low level DVFM primitive functions written specifically
++@                  for the Monahans/Zylonite processor/platform.
++@
++@******************************************************************************
++
++
++@
++@ List of primitive functions in this module:
++@
++    .global enter_d0cs_a
++    .global exit_d0cs_a
++    .global pxa_clkcfg_read
++    .global pxa_clkcfg_write
++
++.equ     CLKMGR_ACCR_OFFSET,0x0000
++.equ     CLKMGR_ACSR_OFFSET,0x0004
++
++.equ     DMEMC_MDCNFG_OFFSET, 0x0000
++.equ     DMEMC_DDRHCAL_OFFSET,0x0060
++
++    .text
++
++@
++@
++@ UINT32  enter_d0cs_a
++@
++@
++@ Description:
++@ 	put system into D0CS mode.
++@
++@ Input Parameters:
++@       r0 - arg1, the address of Clock Manager Controller
++@	r1 - arg2, the address of Dynamic Memory controller
++@ Returns:
++@	r0 - success (0) or failure(1)
++@
++@ Registers Modified:
++@	ACCR, MDCNFG, DDR_HCAL
++@	General Purpose Registers Modified: r3, r4
++@
++@ NOTE:
++@
++
++enter_d0cs_a:
++	stmfd	sp!, {r3, r4, lr}
++	@
++	@ return directly if current mode is D0CS already
++	@
++	ldr     r3, [r0, #CLKMGR_ACSR_OFFSET]   @ load ACSR
++	tst     r3, #0x04000000
++	movne   r0, #0
++	bne     6f
++0:
++	@
++	@ set DMEMC.MDCFG[29]
++	@
++	ldr     r3, [r1, #DMEMC_MDCNFG_OFFSET]	@ get MDCNFG
++	orr     r3, r3, #0x20000000		@ Set DMEMC.MDCNFG[29].
++	str     r3, [r1, #DMEMC_MDCNFG_OFFSET]	@ load MDCNFG
++1:
++	ldr     r3, [r1, #DMEMC_MDCNFG_OFFSET]	@ ensure DMEMC.MDCNFG[29] bit is written
++	tst     r3, #0x20000000
++	beq     1b
++
++	@
++	@ clear DMEMC.DDR_HCAL[31]
++	@
++	ldr     r3, [r1, #DMEMC_DDRHCAL_OFFSET]	@ get DDR_HCAL
++	bic     r3, r3, #0x80000000 		@ Insure DDR_HCAL[31] is clear
++	str     r3, [r1, #DMEMC_DDRHCAL_OFFSET]	@ load DDR_HCAL
++2:
++	ldr     r3, [r1, #DMEMC_DDRHCAL_OFFSET]	@ Insure DDR_HCAL[31] is clear
++	tst     r3, #0x80000000
++	bne     2b
++
++	@
++	@ set ACCR[D0CS] bit
++	@
++	ldr     r3, [r0, #CLKMGR_ACCR_OFFSET]	@ get ACCR
++	orr     r3, r3, #0x04000000		@ set D0CS bit in ACCR
++	str	r3, [r0, #CLKMGR_ACCR_OFFSET]   @ load ACCR
++3:
++	ldr     r3, [r0, #CLKMGR_ACCR_OFFSET]   @ ensure D0CS bit is written
++	tst     r3, #0x04000000
++	beq     3b
++
++	@
++	@ enter D0CS mode
++	@
++	mov     r4, #5              		@ r4: power mode
++	b       enterd0cs           		@ skip the garbage before .align 5
++	.align 5
++enterd0cs:
++	mcr     p14, 0, r4, c7, c0, 0		@ enter D0CS mode
++4:				 		@ wait for system to enter D0CS really
++	ldr     r3, [r0, #CLKMGR_ACSR_OFFSET]   @ load ACSR
++	tst     r3, #0x04000000
++	beq     4b
++5:				 		@ wait for DMEMC.MDCNFG[29] clear
++	ldr     r3, [r1, #DMEMC_MDCNFG_OFFSET]
++	tst     r3, #0x20000000
++	bne     5b
++
++6:
++	@
++	@ return
++	@
++	mov     r0, #0
++	ldmfd   sp!, {r3, r4, pc}		@ return
++
++@
++@
++@ UINT32  exit_d0cs_a
++@
++@
++@ Description:
++@ 	let system exit D0CS mode.
++@
++@       r0 - arg1, the address of Clock Manager Controller
++@	r1 - arg2, the address of Dynamic Memory controller
++@ Returns:
++@	r0 - success (0) or failure(1)
++@
++@ Registers Modified:
++@	ACCR, MDCNFG, DDR_HCAL
++@	General Purpose Registers Modified: r3, r4
++@
++@ NOTE:
++@
++
++exit_d0cs_a:
++	stmfd	sp!, {r3,r4,lr}
++	@
++	@ return directly if current mode is not D0CS
++	@
++	ldr     r3, [r0, #CLKMGR_ACSR_OFFSET]   @ load ACSR
++	tst     r3, #0x04000000
++	beq     6f
++0:
++	@
++	@ set DMEMC.MDCFG[29]
++	@
++	ldr     r3, [r1, #DMEMC_MDCNFG_OFFSET]	@ get MDCNFG
++	orr     r3, r3, #0x20000000 		@ Set DMEMC.MDCNFG[29].
++	str     r3, [r1, #DMEMC_MDCNFG_OFFSET]	@ load MDCNFG
++1:
++	ldr     r3, [r1, #DMEMC_MDCNFG_OFFSET]	@ ensure DMEMC.MDCNFG[29] bit is written
++	tst     r3, #0x20000000
++	beq     1b
++
++	@
++	@ set DMEMC.DDR_HCAL[31]
++	@
++	ldr     r3, [r1, #DMEMC_DDRHCAL_OFFSET]	@ get DDR_HCAL
++	orr     r3, r3, #0x80000000 		@ Insure DDR_HCAL[31] is set
++	str     r3, [r1, #DMEMC_DDRHCAL_OFFSET]	@ load DDR_HCAL
++2:
++	ldr     r3, [r1, #DMEMC_DDRHCAL_OFFSET]	@ Insure DDR_HCAL[31] is set
++	tst     r3, #0x80000000
++	beq     2b
++
++	@
++	@ clear ACCR[D0CS] bit
++	@
++	ldr     r3, [r0, #CLKMGR_ACCR_OFFSET]	@ get ACCR
++	bic     r3, r3, #0x04000000		@ clear D0CS bit in ACCR
++	str	r3, [r0, #CLKMGR_ACCR_OFFSET]   @ load ACCR
++3:
++	ldr     r3, [r0, #CLKMGR_ACCR_OFFSET]   @ ensure D0CS bit is clear
++	tst     r3, #0x04000000
++	bne     3b
++
++	@
++	@ exit D0CS mode
++	@
++	mov     r4, #5              		@ r4: power mode
++	b       exitd0cs           		@ skip the garbage before .align 5
++	.align 5
++exitd0cs:
++	mcr     p14, 0, r4, c7, c0, 0		@ exit D0CS mode
++4:				 		@ wait for system to exit D0CS really
++	ldr     r3, [r0, #CLKMGR_ACSR_OFFSET]   @ load ACSR
++	tst     r3, #0x04000000
++	bne     4b
++5:				 		@ wait for DMEMC.MDCNFG[29] clear
++	ldr     r3, [r1, #DMEMC_MDCNFG_OFFSET]
++	tst     r3, #0x20000000
++	bne     5b
++6:
++	@
++	@ return
++	@
++	mov     r0, #0
++	ldmfd   sp!, {r3,r4,pc}			@ return
++
++@
++@ UINT32 pxa_clkcfg_read
++@
++@ Description:
++@ 	This routine reads the designated PMU register via CoProcesser 14.
++@
++@ Input Parameters:
++@
++@ Returns:
++@	r0 - clkcfg value
++@
++@ Registers Modified:
++@	CoProcessor Register Modified: None
++@	General Purpose Registers Modified: None
++@
++@
++
++pxa_clkcfg_read:
++	mrc  p14, 0, r0, c6, c0, 0		@ Read clkcfg
++	bx lr          @ return
++
++
++
++@
++@ void pxa_clkcfg_write
++@
++@ Description:
++@ 	This routine writes to the designated ClkCFG register via CoProcesser 14.
++@
++@ Input Parameters:
++@       r0 - arg1 - Value to write to ClkCFG register
++@
++
++@ Returns:
++@	None
++@
++@ Registers Modified:
++@	CoProcessor Register Modified: ClkCFG Register
++@	General Purpose Registers Modified: None
++@
++@ NOTE
++@	Error checking not included
++@
++
++pxa_clkcfg_write:
++	mcr	 p14, 0, r0, c6, c0, 0	@ Write ClkCFG
++	bx	 lr         @ return
++
+diff -ur linux-2.6.32/arch/arm/mach-pxa/pxa3xx_pmic.c kernel/arch/arm/mach-pxa/pxa3xx_pmic.c
+--- linux-2.6.32/arch/arm/mach-pxa/pxa3xx_pmic.c	2009-12-13 13:00:47.651947246 +0200
++++ kernel/arch/arm/mach-pxa/pxa3xx_pmic.c	2009-12-12 16:09:26.482948915 +0200
+@@ -0,0 +1,394 @@
++/*
++ * Monahans PMIC abstrction layer
++ *
++ * This software program is licensed subject to the GNU General Public License
++ * (GPL).Version 2,June 1991, available at http://www.fsf.org/copyleft/gpl.html
++
++ * (C) Copyright 2007 Marvell International Ltd.
++ * All Rights Reserved
++ */
++
++#include <mach/pxa3xx_pmic.h>
++
++#include <mach/mfp.h>
++static struct pmic_ops *pxa3xx_pmic_ops;
++
++#ifdef	DEBUG
++/* calculate the elapsed time on operating PMIC */
++static unsigned int start_time, end_time;
++void start_calc_time(void)
++{
++	start_time = OSCR;
++}
++
++void end_calc_time(void)
++{
++	unsigned int time;
++	end_time = OSCR
++	time = (end_time - start_time) * 100 / 325;
++
++	pr_debug("\n%s:\t:%dus\n", __func__, time);
++}
++#else
++void start_calc_time(void) {}
++void end_calc_time(void) {}
++#endif
++
++void pmic_set_ops(struct pmic_ops *ops)
++{
++	printk("pmic_set_ops:%x\n", ops);
++	if (pxa3xx_pmic_ops != NULL) {
++		printk(KERN_ERR "set pmic_ops when pmic_ops is not NULL\n");
++		return;
++	}
++	pxa3xx_pmic_ops = ops;
++	INIT_LIST_HEAD(&pxa3xx_pmic_ops->list);
++	spin_lock_init(&pxa3xx_pmic_ops->cb_lock);
++}
++
++/*****************************************************************************
++ *			Operation of PMIC				     *
++ *****************************************************************************/
++int check_pmic_ops(void)
++{
++	if (!pxa3xx_pmic_ops) {
++		printk(KERN_WARNING "No pmic_ops registered!\n");
++		return -EINVAL;
++	} else
++		return 0;
++}
++
++int pxa3xx_pmic_get_voltage(int cmd, int *pval)
++{
++	int ret;
++
++	ret = check_pmic_ops();
++	if (ret < 0)
++		return ret;
++
++	if (pxa3xx_pmic_ops->get_voltage)
++		return pxa3xx_pmic_ops->get_voltage(cmd, pval);
++	else
++		return -EINVAL;
++}
++EXPORT_SYMBOL(pxa3xx_pmic_get_voltage);
++
++int pxa3xx_pmic_set_voltage(int cmd, int val)
++{
++	int ret;
++
++	ret = check_pmic_ops();
++	if (ret < 0)
++		return ret;
++
++	if (pxa3xx_pmic_ops->set_voltage)
++		return pxa3xx_pmic_ops->set_voltage(cmd, val);
++	else
++		return -EINVAL;
++}
++EXPORT_SYMBOL(pxa3xx_pmic_set_voltage);
++
++int pxa3xx_pmic_check_voltage(int cmd)
++{
++	int ret;
++
++	ret = check_pmic_ops();
++	if (ret < 0)
++		return ret;
++
++	if (pxa3xx_pmic_ops->check_voltage)
++		return pxa3xx_pmic_ops->check_voltage(cmd);
++	else
++		return -EINVAL;
++}
++EXPORT_SYMBOL(pxa3xx_pmic_check_voltage);
++
++int pxa3xx_pmic_enable_voltage(int cmd, int enable)
++{
++	int ret;
++
++	ret = check_pmic_ops();
++	if (ret < 0)
++		return ret;
++
++	if (pxa3xx_pmic_ops->enable_voltage)
++		return pxa3xx_pmic_ops->enable_voltage(cmd, enable);
++	else
++		return -EINVAL;
++}
++EXPORT_SYMBOL(pxa3xx_pmic_enable_voltage);
++
++int pxa3xx_pmic_enable_led(int cmd, int enable)
++{
++	int ret;
++
++	ret=check_pmic_ops();
++	if (ret > 0)
++		return ret;
++
++	if(pxa3xx_pmic_ops->enable_led)
++		return pxa3xx_pmic_ops->enable_led(cmd, enable);
++	else
++		return -EINVAL;
++}
++
++EXPORT_SYMBOL(pxa3xx_pmic_enable_led);
++
++int pxa3xx_pmic_is_vbus_assert(void)
++{
++	int ret;
++
++	ret = check_pmic_ops();
++	if (ret < 0) /* If illegal pmic_ops, always no vbus activity */
++		return 0;
++
++	if (pxa3xx_pmic_ops->is_vbus_assert)
++		return pxa3xx_pmic_ops->is_vbus_assert();
++
++	return 0;
++}
++EXPORT_SYMBOL(pxa3xx_pmic_is_vbus_assert);
++
++int pxa3xx_pmic_is_avbusvld(void)
++{
++	int ret;
++
++	ret = check_pmic_ops();
++	if (ret < 0) /* If illegal pmic_ops, always no A vbus valid */
++		return 0;
++
++	if (pxa3xx_pmic_ops->is_avbusvld)
++		return pxa3xx_pmic_ops->is_avbusvld();
++
++	return 0;
++}
++EXPORT_SYMBOL(pxa3xx_pmic_is_avbusvld);
++
++int pxa3xx_pmic_is_asessvld(void)
++{
++	int ret;
++
++	ret = check_pmic_ops();
++	if (ret < 0) /* If illegal pmic_ops, always no A assert valid */
++		return 0;
++
++	if (pxa3xx_pmic_ops->is_asessvld)
++		return pxa3xx_pmic_ops->is_asessvld();
++
++	return 0;
++}
++EXPORT_SYMBOL(pxa3xx_pmic_is_asessvld);
++
++int pxa3xx_pmic_is_bsessvld(void)
++{
++	int ret;
++
++	ret = check_pmic_ops();
++	if (ret < 0) /* If illegal pmic_ops, always no B assert valid */
++		return 0;
++
++	if (pxa3xx_pmic_ops->is_bsessvld)
++		return pxa3xx_pmic_ops->is_bsessvld();
++
++	return 0;
++}
++EXPORT_SYMBOL(pxa3xx_pmic_is_bsessvld);
++
++int pxa3xx_pmic_is_srp_ready(void)
++{
++	int ret;
++
++	ret = check_pmic_ops();
++	if (ret < 0) /* If illegal pmic_ops, always no SRP detect */
++		return 0;
++
++	if (pxa3xx_pmic_ops->is_srp_ready)
++		return pxa3xx_pmic_ops->is_srp_ready();
++
++	return 0;
++
++}
++EXPORT_SYMBOL(pxa3xx_pmic_is_srp_ready);
++
++int pxa3xx_pmic_set_pump(int enable)
++{
++	int ret;
++
++	ret = check_pmic_ops();
++	if (ret < 0)
++		return ret;
++
++	if (pxa3xx_pmic_ops->set_pump)
++		return pxa3xx_pmic_ops->set_pump(enable);
++
++	return 0;
++}
++EXPORT_SYMBOL(pxa3xx_pmic_set_pump);
++
++int pxa3xx_pmic_set_vbus_supply(int enable, int srp)
++{
++	int ret;
++
++	ret = check_pmic_ops();
++	if (ret < 0)
++		return ret;
++
++	if (pxa3xx_pmic_ops->set_vbus_supply)
++		return pxa3xx_pmic_ops->set_vbus_supply(enable, srp);
++
++	return 0;
++}
++EXPORT_SYMBOL(pxa3xx_pmic_set_vbus_supply);
++
++int pxa3xx_pmic_set_usbotg_a_mask(void)
++{
++	int ret;
++
++	ret = check_pmic_ops();
++	if (ret < 0)
++		return ret;
++
++	if (pxa3xx_pmic_ops->set_usbotg_a_mask)
++		return pxa3xx_pmic_ops->set_usbotg_a_mask();
++
++	return 0;
++}
++EXPORT_SYMBOL(pxa3xx_pmic_set_usbotg_a_mask);
++
++int pxa3xx_pmic_set_usbotg_b_mask(void)
++{
++	int ret;
++
++	ret = check_pmic_ops();
++	if (ret < 0)
++		return ret;
++
++	if (pxa3xx_pmic_ops->set_usbotg_b_mask)
++		return pxa3xx_pmic_ops->set_usbotg_b_mask();
++
++	return 0;
++}
++EXPORT_SYMBOL(pxa3xx_pmic_set_usbotg_b_mask);
++
++int pxa3xx_pmic_is_onkey_assert(void)
++{
++        int ret;
++
++        ret = check_pmic_ops();
++        if (ret < 0)
++                return ret;
++
++        if (pxa3xx_pmic_ops->is_onkey_assert)
++                return pxa3xx_pmic_ops->is_onkey_assert();
++
++        return 0;
++}
++EXPORT_SYMBOL(pxa3xx_pmic_is_onkey_assert);
++
++/* Register pmic callback */
++int pmic_callback_register(unsigned long event,
++	       void (*func)(unsigned long event))
++{
++	int ret;
++	unsigned long flags;
++	struct pmic_callback *pmic_cb;
++
++	might_sleep();
++
++	ret = check_pmic_ops();
++	if (ret < 0)
++		return ret;
++
++	pmic_cb = kzalloc(sizeof(*pmic_cb), GFP_KERNEL);
++	if (!pmic_cb)
++		return -ENOMEM;
++
++	INIT_LIST_HEAD(&pmic_cb->list);
++	pmic_cb->event = event;
++	pmic_cb->func = func;
++
++	spin_lock_irqsave(&pxa3xx_pmic_ops->cb_lock, flags);
++	list_add(&pmic_cb->list, &pxa3xx_pmic_ops->list);
++	spin_unlock_irqrestore(&pxa3xx_pmic_ops->cb_lock, flags);
++
++	return 0;
++}
++EXPORT_SYMBOL(pmic_callback_register);
++
++/* Unregister pmic callback */
++int pmic_callback_unregister(unsigned long event,
++		void (*func)(unsigned long event))
++{
++	unsigned long flags;
++	struct pmic_callback *pmic_cb, *next;
++
++	spin_lock_irqsave(&pxa3xx_pmic_ops->cb_lock, flags);
++	list_for_each_entry_safe(pmic_cb, next, &pxa3xx_pmic_ops->list, list) {
++		if ((pmic_cb->event == event) && (pmic_cb->func == func)) {
++			list_del_init(&pmic_cb->list);
++			kfree(pmic_cb);
++		}
++	}
++	spin_unlock_irqrestore(&pxa3xx_pmic_ops->cb_lock, flags);
++	return 0;
++}
++EXPORT_SYMBOL(pmic_callback_unregister);
++
++int pmic_event_handle(unsigned long event)
++{
++	int ret;
++	unsigned long flags;
++	struct pmic_callback *pmic_cb;
++	ret = check_pmic_ops();
++	if (ret < 0)
++		return ret;
++
++	spin_lock_irqsave(&pxa3xx_pmic_ops->cb_lock, flags);
++	list_for_each_entry(pmic_cb, &pxa3xx_pmic_ops->list, list) {
++		spin_unlock_irqrestore(&pxa3xx_pmic_ops->cb_lock, flags);
++		/* event is bit-wise parameter, need bit AND here as filter */
++		if ((pmic_cb->event & event) && (pmic_cb->func))
++			pmic_cb->func(event);
++		spin_lock_irqsave(&pxa3xx_pmic_ops->cb_lock, flags);
++	}
++	spin_unlock_irqrestore(&pxa3xx_pmic_ops->cb_lock, flags);
++	return 0;
++}
++EXPORT_SYMBOL(pmic_event_handle);
++
++
++int px3xx_pmic_event_enable(unsigned long event, int enable)
++{
++	int ret;
++	u8 val;
++	unsigned long flags;
++	struct pmic_callback *pmic_cb;
++	ret = check_pmic_ops();
++	if (ret < 0)
++		return ret;
++	printk("pxa pmic event enable 11\n");
++        if(pxa3xx_pmic_ops->enable_event)
++	{
++		printk("pxa pmic event enable 22\n");
++                return pxa3xx_pmic_ops->enable_event(event, enable);
++	}
++        else
++                return -EINVAL;
++}
++EXPORT_SYMBOL(px3xx_pmic_event_enable);
++
++int pxa3xx_pmic_is_hookswitch_assert(void)
++{
++        int ret;
++
++        ret = check_pmic_ops();
++        if (ret < 0)
++                return ret;
++
++        if (pxa3xx_pmic_ops->is_hookswitch_assert)
++                return pxa3xx_pmic_ops->is_hookswitch_assert();
++
++        return 0; 
++}
++EXPORT_SYMBOL(pxa3xx_pmic_is_hookswitch_assert);
++
+diff -ur linux-2.6.32/arch/arm/mach-pxa/sgh_i780_i900.c kernel/arch/arm/mach-pxa/sgh_i780_i900.c
+--- linux-2.6.32/arch/arm/mach-pxa/sgh_i780_i900.c	2009-12-13 13:00:53.329024629 +0200
++++ kernel/arch/arm/mach-pxa/sgh_i780_i900.c	2009-12-12 16:09:26.486282481 +0200
+@@ -0,0 +1,618 @@
++/**
++ * Support for the PXA311 and PXA312 based Samsung SGH devices
++ * 		m480, i780, i900, i904, i908, i910
++ *
++ * Copyright (C) 2009 Sacha Refshauge <xsacha@gmail.com>
++ * Copyright (C) 2009 Stefan Schmidt <stefan@datenfreihafen.org>
++ * Copyright (C) 2009 Mustafa Ozsakalli <ozsakalli@hotmail.com>
++ *
++ * Based on zylonite.c Copyright (C) 2006 Marvell International Ltd.
++ *
++ * This program is free software; you can redistribute it and/or modify
++ * it under the terms of the GNU General Public License version 2 as
++ * published by the Free Software Foundation.
++ */
++
++#include <linux/module.h>
++#include <linux/kernel.h>
++#include <linux/delay.h>
++#include <linux/interrupt.h>
++#include <linux/init.h>
++#include <linux/platform_device.h>
++#include <linux/gpio.h>
++#include <linux/pwm_backlight.h>
++#include <linux/power_supply.h>
++#include <linux/pda_power.h>
++#include <linux/spi/spi.h>
++#include <linux/spi/libertas_spi.h>
++#include <../drivers/staging/android/timed_gpio.h>
++
++#include <plat/i2c.h>
++#include <asm/mach-types.h>
++#include <asm/mach/arch.h>
++#include <mach/hardware.h>
++#include <mach/pxafb.h>
++#include <mach/audio.h>
++#include <mach/mmc.h>
++#include <mach/udc.h>
++#include <mach/ohci.h>
++#include <mach/pxa27x-udc.h>
++#include <mach/pxa27x_keypad.h>
++#include <mach/pxa2xx_spi.h>
++#include <mach/pxa3xx-regs.h>
++#include <mach/mfp-pxa300.h>
++#if defined(CONFIG_PXA_DVFM)
++#include <mach/dvfm.h>
++#include <mach/pxa3xx_dvfm.h> 
++#include <mach/pmu.h>
++#endif
++
++#include <mach/sgh_msm6k.h>
++
++#include "devices.h"
++#include "generic.h"
++
++#define SGH_BATT_I2C_SLAVE_ADDRESS		0x34
++
++#define GPIO09_SGH_LED_GREEN		  	  9
++#define GPIO23_SGH_TOUCHSCREEN		 	 23
++#define GPIO71_SGH_LED_BLUE		 	 71
++#define GPIO79_SGH_LED_VIBRATE		 	 79
++#define GPIO88_SGH_BATT_CHARGE		 	 88
++#define GPIO104_SGH_WIFI_CMD		   	104
++#define GPIO105_SGH_CARD_DETECT		   	105
++
++#define GPIO18_SGH_I780_WIFI_CMD	 	 11
++#define GPIO19_SGH_I780_SPK_AUDIO	 	 19
++#define GPIO48_SGH_I780_LED_BACKLIGHT		 48
++#define GPIO75_SGH_I780_LED_RED		 	 75
++#define GPIO94_SGH_I780_WIFI_POWER	 	 94
++
++#define GPIO03_SGH_I900_WIFI_POWER	  	  3
++#define GPIO17_SGH_I900_SPK_AUDIO	 	 17
++#define GPIO48_SGH_I900_LED_RED		 	 48
++#define GPIO76_SGH_I900_BT_POWER	 	 76
++#define GPIO118_SGH_I900_WIFI_CMD	   	118
++
++#define GPIO16_SGH_SPI_CHIP_SEL	 	 	 16
++
++#if defined(CONFIG_LEDS_GPIO) || defined(CONFIG_LEDS_GPIO_MODULE)
++static struct gpio_led sgh_leds[] = {
++	[0] = {
++		.name			= "red",
++	},
++	[1] = {
++		.name			= "green",
++		.default_trigger	= "mmc0",
++		.gpio			= GPIO09_SGH_LED_GREEN,
++	},
++	[2] = {
++		.name			= "blue",
++		.default_trigger	= "mmc0",
++		.gpio			= GPIO71_SGH_LED_BLUE,
++	},
++	[3] = {
++		.name			= "keyboard",
++	},
++};
++
++static struct gpio_led_platform_data sgh_leds_info = {
++	.leds		= sgh_leds,
++	.num_leds	= ARRAY_SIZE(sgh_leds),
++};
++
++static struct platform_device sgh_device_leds = {
++	.name		= "leds-gpio",
++	.id		= -1,
++	.dev		= {
++		.platform_data = &sgh_leds_info,
++	}
++};
++
++static struct timed_gpio sgh_vibrator = {
++         .name                   = "vibrator",
++         .gpio                   = GPIO79_SGH_LED_VIBRATE,
++         .max_timeout            = 1000,
++};
++
++static struct timed_gpio_platform_data sgh_vibrator_info = {
++        .gpios          = &sgh_vibrator,
++        .num_gpios      = 1,
++};
++
++static struct platform_device sgh_device_vibrator = {
++        .name           = "timed-gpio",
++        .id             = -1,
++        .dev            = {
++                .platform_data = &sgh_vibrator_info,
++        }
++};
++
++static void __init sgh_init_leds(void)
++{
++
++	sgh_leds[0].gpio = (machine_is_sgh_i780()) ? GPIO75_SGH_I780_LED_RED : GPIO48_SGH_I900_LED_RED;
++	if(machine_is_sgh_i780())
++		sgh_leds[3].gpio = GPIO48_SGH_I780_LED_BACKLIGHT;
++
++	platform_device_register(&sgh_device_leds);
++	//timed_gpio doesnt request gpio
++	gpio_request(GPIO79_SGH_LED_VIBRATE, "SGH-VIBRATOR");
++	platform_device_register(&sgh_device_vibrator);
++
++}
++#else
++static inline void sgh_init_leds(void) {}
++#endif
++
++#if defined(CONFIG_FB_PXA) || defined(CONFIG_FB_PXA_MODULE)
++static struct platform_pwm_backlight_data sgh_backlight_data = {
++	.pwm_id		= 3,
++	.max_brightness	= 100,
++	.dft_brightness	= 100,
++	.pwm_period_ns	= 10000,
++};
++
++static struct platform_device sgh_backlight_device = {
++	.name		= "backlight",
++	.dev		= {
++		.parent = &pxa27x_device_pwm1.dev,
++		.platform_data	= &sgh_backlight_data,
++	},
++};
++/* Pixclock Calculation
++ Calculated from reviewing HaRET source: http://xanadux.cvs.sourceforge.net/viewvc/xanadux/haret/haret-gnu/src/script.cpp?view=markup
++ pixclock = K * 8MHz / CLK ;   where CLK is 312MHz and K is last 8 bits of lccr3
++
++ New: pixclock = (K * 200000000) / 15600
++*/
++static struct pxafb_mode_info sgh_i780_mode = {
++        .pixclock		= 243600,	// K = 19
++        .xres			= 320,		// HACK: Android does not like square resolutions
++        .yres			= 319,
++        .bpp			= 16,
++        .hsync_len		= 16,
++        .left_margin		= 24,
++        .right_margin		= 24,
++        .vsync_len		= 2,
++        .upper_margin		= 3,
++        .lower_margin   	= 0,
++        .sync          	 = 0,
++};
++static struct pxafb_mode_info sgh_i900_mode = {
++	.pixclock		= 256500,	// K = 20
++	.xres			= 240,
++	.yres			= 400,
++	.bpp			= 16,
++	.hsync_len		= 8,
++	.left_margin		= 8,
++	.right_margin		= 8,
++	.vsync_len		= 4,
++	.upper_margin		= 38,
++	.lower_margin		= 38,
++	.sync			= 0, //FB_SYNC_VERT_HIGH_ACT,
++};
++
++static struct pxafb_mach_info sgh_lcd_info = {
++	.num_modes		= 1,
++	.lcd_conn		= LCD_COLOR_TFT_16BPP | LCD_PCLK_EDGE_FALL,
++};
++
++static void __init sgh_init_lcd(void)
++{
++	platform_device_register(&sgh_backlight_device);
++	sgh_lcd_info.modes = (machine_is_sgh_i780()) ? &sgh_i780_mode : &sgh_i900_mode;
++	set_pxa_fb_info(&sgh_lcd_info);
++}
++#else
++static inline void sgh_init_lcd(void) {}
++#endif
++
++/****************************
++* Keypad                    *
++****************************/
++
++/*Android (i.e. non-linux) keys:
++Name:            defined as:   function:
++KEY_SEND         231           Send key
++KEY_END          107           End key
++KEY_BACK         158           Go back a page
++KEY_MENU         139           Open a special menu
++KEY_HOME         102           Return to the home screen
++KEY_SEARCH       217           Open the Android search
++KEY_VOLUMEUP     115           Increase volume
++KEY_VOLUMEDOWN   114           Decrease volume
++KEY_CAMERA       212           Opens camera
++KEY_CAMERAFOCUS  211           Focuses camera (Omnia only, replaces KEY_HP in kernel/include/linux/input.h)
++*/
++
++#if defined(CONFIG_KEYBOARD_PXA27x) || defined(CONFIG_KEYBOARD_PXA27x_MODULE)
++/* KEY(row, col, key_code) */
++static unsigned int sgh_i780_matrix_key_map[] = {
++/* QWERTY Keyboard */
++/* 1st row */
++KEY(0, 0, KEY_Q), KEY(7, 1, KEY_W), KEY(2, 0, KEY_E), KEY(3, 0, KEY_R), KEY(4, 0, KEY_T),
++KEY(0, 4, KEY_Y), KEY(1, 4, KEY_U), KEY(2, 4, KEY_I), KEY(3, 4, KEY_O), KEY(4, 4, KEY_P),
++/* 2nd row */
++KEY(0, 1, KEY_A), KEY(7, 2, KEY_S), KEY(2, 1, KEY_D), KEY(3, 1, KEY_F), KEY(4, 1, KEY_G),
++KEY(0, 5, KEY_H), KEY(1, 5, KEY_J), KEY(2, 5, KEY_K), KEY(3, 5, KEY_L), KEY(4, 5, KEY_BACKSPACE),
++/* 3rd row */
++KEY(0, 2, KEY_LEFTALT), KEY(1, 2, KEY_Z), KEY(2, 2, KEY_X), KEY(3, 2, KEY_C), KEY(4, 2, KEY_V),
++KEY(0, 6, KEY_B), KEY(1, 6, KEY_N), KEY(2, 6, KEY_M), KEY(3, 6, KEY_DOT), KEY(4, 6, KEY_ENTER),
++/* 4th row */
++KEY(0, 3, KEY_LEFTSHIFT), KEY(1, 3, KEY_RIGHTALT), KEY(2, 3, KEY_0), KEY(3, 3, KEY_SPACE),
++KEY(4, 3, KEY_COMMA), KEY(7, 6, KEY_SLASH), /* Message */ KEY(5, 1, KEY_TAB), /* GPS */
++
++/* Volume Keys */
++KEY(1, 0, KEY_VOLUMEUP),
++KEY(1, 1, KEY_VOLUMEDOWN),
++
++/* Left Softkey */      /* Windows Key */       /* OK */        /* Right Softkey */
++KEY(5, 4, KEY_MINUS), KEY(5, 2, KEY_MENU), KEY(5, 3, KEY_EXIT),  KEY(5, 6, KEY_F2),
++KEY(5, 5, KEY_SEND),            KEY(6, 4, KEY_REPLY),           KEY(7, 0, KEY_END),
++/* Green Key */                  /* Center */                   /* Red Key */
++
++/* Camera */
++KEY(7, 3, KEY_CAMERA),
++};
++
++static unsigned int sgh_i900_matrix_key_map[] = {
++	/* KEY(row, col, key_code) */
++	KEY(0, 0, KEY_CAMERAFOCUS),	//Camera half-press
++	KEY(0, 1, KEY_CAMERA),		//Camera full-press
++	KEY(0, 2, KEY_ENTER),		//Center optical dpad button
++	KEY(1, 0, KEY_VOLUMEUP),	//Volume up
++	KEY(1, 1, KEY_VOLUMEDOWN),	//Volume down
++	KEY(1, 2, KEY_SEND),		//Send key
++	KEY(2, 0, KEY_MENU),		//Top right key (Main Menu button)
++	KEY(2, 1, KEY_END),		//???
++	KEY(2, 2, KEY_BACK),		//End key (Back button)
++       
++};
++
++static struct pxa27x_keypad_platform_data sgh_keypad_info = {
++	.enable_rotary0		= 0,
++
++	.debounce_interval	= 30,
++};
++
++static void __init sgh_init_keypad(void)
++{
++	if(machine_is_sgh_i780())
++	{
++		sgh_keypad_info.matrix_key_rows = 8;
++		sgh_keypad_info.matrix_key_cols = 7;
++		sgh_keypad_info.matrix_key_map = sgh_i780_matrix_key_map;
++		sgh_keypad_info.matrix_key_map_size = ARRAY_SIZE(sgh_i780_matrix_key_map);
++	}
++	else
++	{
++		sgh_keypad_info.matrix_key_rows = 3;
++		sgh_keypad_info.matrix_key_cols = 3;
++		sgh_keypad_info.matrix_key_map = sgh_i900_matrix_key_map;
++		sgh_keypad_info.matrix_key_map_size = ARRAY_SIZE(sgh_i900_matrix_key_map);
++	}
++
++	pxa_set_keypad_info(&sgh_keypad_info);
++}
++#else
++static inline void sgh_init_keypad(void) {}
++#endif
++
++#if defined(CONFIG_MMC)
++static int sgh_mci_sdcard_init(struct device *dev,
++			     irq_handler_t sgh_detect_int,
++			     void *data)
++{
++	int err, cd_irq;
++	int gpio_cd = GPIO105_SGH_CARD_DETECT;
++
++	cd_irq = gpio_to_irq(gpio_cd);
++
++	/*
++	 * setup GPIO for MMC controller
++	 */
++	err = gpio_request(gpio_cd, "microSD card detect");
++	if (err)
++		goto err_request_cd;
++	gpio_direction_input(gpio_cd);
++
++	err = request_irq(cd_irq, sgh_detect_int,
++			  IRQF_TRIGGER_RISING | IRQF_TRIGGER_FALLING,
++			  "microSD card detect", data);
++	if (err) {
++		printk(KERN_ERR "%s: MicroSD: "
++				"can't request card detect IRQ\n", __func__);
++		goto err_request_cd;
++	}
++
++	return 0;
++
++err_request_cd:
++	return err;
++}
++
++static void sgh_mci_sdcard_exit(struct device *dev, void *data)
++{
++	int cd_irq, gpio_cd;
++
++	cd_irq = gpio_to_irq(105);
++	gpio_cd = 105;
++
++	free_irq(cd_irq, data);
++	gpio_free(gpio_cd);
++}
++
++static struct pxamci_platform_data sgh_mci_sdcard_platform_data = {
++	.detect_delay	= 20,
++	.ocr_mask	= MMC_VDD_32_33|MMC_VDD_33_34,
++	.init		= sgh_mci_sdcard_init,
++	.exit		= sgh_mci_sdcard_exit,
++	.gpio_card_detect	= -1,
++	.gpio_card_ro		= -1,
++	.gpio_power		= -1,
++};
++
++
++static void __init sgh_init_mmc(void)
++{
++	pxa_set_mci_info(&sgh_mci_sdcard_platform_data);		// External MicroSD
++	if(machine_is_sgh_i900())
++		pxa3xx_set_mci2_info(&sgh_mci_sdcard_platform_data);	// Internal MicroSD
++}
++#else
++static inline void sgh_init_mmc(void) {}
++#endif
++static void sgh_udc_command(int cmd)
++{
++        switch (cmd) {
++        case PXA2XX_UDC_CMD_CONNECT:
++                //UP2OCR |=  UP2OCR_HXOE | UP2OCR_DPPUE | UP2OCR_DPPUBE;
++		UP2OCR |= 0xf024;	// USB Port 2 Output Control Register
++                break;
++        case PXA2XX_UDC_CMD_DISCONNECT:
++                //UP2OCR &= ~(UP2OCR_HXOE | UP2OCR_DPPUE | UP2OCR_DPPUBE);
++		UP2OCR &= 0xf024;
++                break;
++        }
++}
++static struct pxa2xx_udc_mach_info sgh_udc_info __initdata = {
++        .udc_command            = sgh_udc_command,
++};
++	/* WinMo: UHCHR_SSEP2 | UHCHR_SSEP1 | UHCHR_SSE | UHCHR_CGR | UHCHR_FHR
++           Set the Power Control Polarity Low */
++/*        UHCHR = (UHCHR | UHCHR_PCPL) &
++                ~(UHCHR_SSEP1 | UHCHR_SSEP2 | UHCHR_SSE);
++*/
++static int sgh_init_udc(void)
++{
++        pxa_set_udc_info(&sgh_udc_info);
++        return 0;
++}
++
++#if defined(CONFIG_USB_OHCI_HCD) || defined(CONFIG_USB_OHCI_HCD_MODULE)
++static int sgh_ohci_init(struct device *dev)
++{
++	return 0;
++}
++static struct pxaohci_platform_data sgh_ohci_platform_data = {
++	.port_mode	= PMM_PERPORT_MODE,
++	.init		= sgh_ohci_init
++};
++
++static void __init sgh_init_ohci(void)
++{
++	pxa_set_ohci_info(&sgh_ohci_platform_data);
++}
++#else
++static inline void sgh_init_ohci(void) {}
++#endif /* CONFIG_USB_OHCI_HCD || CONFIG_USB_OHCI_HCD_MODULE */
++
++#if defined(CONFIG_I2C) || defined(CONFIG_I2C_MODULE)
++static struct i2c_board_info __initdata sgh_i2c_board_info[] = {
++        { /* PM6558 Battery */
++                .type = "sgh_battery",
++                .addr = SGH_BATT_I2C_SLAVE_ADDRESS,
++	},
++};
++static void __init sgh_init_i2c(void)
++{
++	i2c_register_board_info(0, sgh_i2c_board_info,
++                                ARRAY_SIZE(sgh_i2c_board_info));
++        pxa_set_i2c_info(NULL);
++}
++#else
++static inline void sgh_init_i2c(void) {}
++#endif
++
++#if defined(CONFIG_SPI_PXA2XX) || defined(CONFIG_SPI_PXA2XX_MASTER)
++static void sgh_spi_wifi_cs(u32 command)
++{
++	gpio_set_value(GPIO16_SGH_SPI_CHIP_SEL, !(command == PXA2XX_CS_ASSERT));
++}
++
++static int sgh_libertas_setup(struct spi_device *spi)
++{
++	int WifiPwr = 0;
++	int WifiCmd = 0;
++	if(machine_is_sgh_i780())
++	{
++		WifiPwr = GPIO94_SGH_I780_WIFI_POWER;
++		WifiCmd = GPIO18_SGH_I780_WIFI_CMD;
++	}
++	else if(machine_is_sgh_i900())
++	{
++		WifiPwr = GPIO03_SGH_I900_WIFI_POWER;
++		WifiCmd = GPIO118_SGH_I900_WIFI_CMD;
++	}
++	gpio_request(WifiPwr,"WLAN");
++	gpio_request(0x10,"WLAN");
++	gpio_request(0x68,"WLAN");
++	gpio_request(WifiCmd,"WLAN");
++
++	//pxa_init_hw
++	gpio_direction_output(0x68,1);
++	gpio_direction_output(WifiCmd,1);
++	gpio_direction_output(WifiPwr,1);
++	gpio_direction_output(0x10,1);
++	mdelay(60);
++
++	gpio_set_value(WifiPwr,1);
++	mdelay(60);
++	gpio_set_value(WifiCmd,1);
++	gpio_set_value(0x10,1);
++	gpio_set_value(0x68,1);
++	mdelay(60);
++
++
++	//gspx_power_up
++	gpio_set_value(WifiPwr,1);
++	mdelay(60);
++	gpio_set_value(WifiCmd,1);
++	gpio_set_value(0x68,1);
++	mdelay(150);
++
++	//gspx_reset_module
++	gpio_set_value(0x68,1);
++	mdelay(60);
++	gpio_set_value(0x68,0);
++	mdelay(60);
++	gpio_set_value(0x68,1);
++	mdelay(100);
++
++	spi->bits_per_word = 16;
++	spi_setup(spi);
++
++	return 0;
++}
++
++static struct pxa2xx_spi_chip sgh_wifi_chip = {
++	.rx_threshold	= 8,
++	.tx_threshold	= 8,
++	.timeout	= 235,
++	.dma_burst_size = 16,
++	.cs_control	= sgh_spi_wifi_cs,
++};
++
++static struct pxa2xx_spi_master sgh_spi_info = {
++	.clock_enable 	= CKEN_SSP1,
++	.num_chipselect	= 1,
++	.enable_dma	= 1,
++};
++
++struct libertas_spi_platform_data sgh_wifi_pdata = {
++	.use_dummy_writes	= 0,
++	.setup			= sgh_libertas_setup,
++};
++
++static struct spi_board_info sgh_spi_devices[] __initdata = {
++	{	//wireless
++		.modalias		= "libertas_spi",
++		.max_speed_hz		= 13000000,
++		.bus_num		= 1,
++		.irq			= IRQ_GPIO(8),
++		.chip_select		= 0,
++		.controller_data	= &sgh_wifi_chip,
++		.platform_data		= &sgh_wifi_pdata,
++	},
++};
++
++static void __init sgh_init_spi(void)
++{
++	sgh_spi_devices[0].irq = IRQ_GPIO(machine_is_sgh_i780() ? 11 : 8);
++	pxa2xx_set_spi_info(1, &sgh_spi_info);
++	spi_register_board_info(ARRAY_AND_SIZE(sgh_spi_devices));
++}
++#else
++static inline void sgh_init_spi(void){}
++#endif
++
++#if defined(CONFIG_PXA_DVFM)
++struct pxa3xx_freq_mach_info sgh_freq_mach_info = {
++	.flags = 0,
++}; 
++
++static void __init sgh_init_dvfm() {
++	set_pxa3xx_freq_info(&sgh_freq_mach_info);
++	pxa3xx_set_pmu_info(NULL);
++}
++#else
++static inline void sgh_init_dvfm(void){}
++#endif
++
++static mfp_cfg_t sgh_mfp_cfg[] __initdata = {
++        /* AC97 */
++	//GPIO23_AC97_nACRESET,
++        GPIO25_AC97_SDATA_IN_0,
++        GPIO27_AC97_SDATA_OUT,
++        GPIO28_AC97_SYNC,
++        GPIO29_AC97_BITCLK,
++
++	/* KEYPAD */
++	GPIO115_KP_MKIN_0 | MFP_LPM_EDGE_BOTH,
++	GPIO116_KP_MKIN_1 | MFP_LPM_EDGE_BOTH,
++	GPIO117_KP_MKIN_2 | MFP_LPM_EDGE_BOTH,
++	GPIO121_KP_MKOUT_0,
++	GPIO122_KP_MKOUT_1,
++	GPIO123_KP_MKOUT_2,
++	GPIO124_KP_MKOUT_3,
++	
++};
++
++static struct platform_device sgh_audio = {
++        .name           = "sgh-asoc",
++        .id             = -1,
++};
++
++static struct platform_device *devices[] __initdata = {
++	&sgh_audio,
++};
++
++
++static void __init sgh_init(void)
++{
++	static int dvfm = 0;
++
++	pxa3xx_mfp_config(ARRAY_AND_SIZE(sgh_mfp_cfg));
++	sgh_init_dvfm();
++
++	rpc_init();
++
++	sgh_init_lcd();
++	sgh_init_mmc();
++	sgh_init_leds();
++	sgh_init_keypad();
++
++	pxa_set_ac97_info(NULL);
++	platform_add_devices(devices, ARRAY_SIZE(devices));
++
++	sgh_init_ohci();
++	sgh_init_udc();
++	sgh_init_i2c();
++	sgh_init_spi();
++	/*
++	dvfm_register("Test", &dvfm);
++	dvfm_disable_op_name("D1", dvfm);
++	dvfm_disable_op_name("D2", dvfm);
++	*/
++}
++
++MACHINE_START(SGH_I780, "Samsung SGH-i780 (Mirage) phone")
++        .phys_io        = 0x40000000,
++        .boot_params    = 0xa0000100,
++        .io_pg_offst    = (io_p2v(0x40000000) >> 18) & 0xfffc,
++        .map_io         = pxa_map_io,
++        .init_irq       = pxa3xx_init_irq,
++        .timer          = &pxa_timer,
++        .init_machine   = sgh_init,
++MACHINE_END
++
++MACHINE_START(SGH_I900, "Samsung SGH-i900 (Omnia) phone")
++	.phys_io	= 0x40000000,
++	.boot_params	= 0xa0000100,
++	.io_pg_offst	= (io_p2v(0x40000000) >> 18) & 0xfffc,
++	.map_io		= pxa_map_io,
++	.init_irq	= pxa3xx_init_irq,
++	.timer		= &pxa_timer,
++	.init_machine	= sgh_init,
++MACHINE_END
+diff -ur linux-2.6.32/arch/arm/mach-pxa/sgh_rpc.c kernel/arch/arm/mach-pxa/sgh_rpc.c
+--- linux-2.6.32/arch/arm/mach-pxa/sgh_rpc.c	2009-12-13 13:00:59.168618858 +0200
++++ kernel/arch/arm/mach-pxa/sgh_rpc.c	2009-12-12 16:09:26.486282481 +0200
+@@ -0,0 +1,333 @@
++/**
++ * Samsung SGH I900 RPC Driver for MSM6K
++ *
++ * Copyright (C) 2009 Mustafa Ozsakalli <ozsakalli@hotmail.com>
++ *
++ * This program is free software; you can redistribute it and/or modify
++ * it under the terms of the GNU General Public License version 2 as
++ * published by the Free Software Foundation.
++ */
++
++#include <linux/module.h>
++#include <linux/kernel.h>
++#include <linux/init.h>
++#include <linux/interrupt.h>
++#include <linux/irq.h>
++#include <linux/miscdevice.h>
++#include <linux/io.h>
++#include <linux/platform_device.h>
++#include <linux/gpio.h>
++#include <linux/delay.h>
++#include <linux/workqueue.h>
++#include <linux/completion.h>
++#include <linux/list.h>
++#include <linux/fs.h>
++#include <linux/cdev.h>
++
++#include <asm/uaccess.h>
++
++#include <mach/hardware.h>
++#include <mach/sgh_msm6k.h>
++
++
++#include "devices.h"
++
++static DEFINE_SPINLOCK(msgs_lock);
++static DEFINE_SPINLOCK(crc_lock);
++
++static void do_read_data(struct work_struct *work);
++static DECLARE_WORK(work_read, do_read_data);
++static struct workqueue_struct *workqueue;
++
++struct class *sgh_rpc_class;
++dev_t sgh_rpc_devno;
++
++static struct cdev rpc_cdev;
++static struct device *rpc_device;
++
++struct __rpc_msg {
++	int command;
++	int type;
++	int index;
++	int len;
++	int crc;
++	char *data;
++
++	struct __rpc_msg *next;
++};
++
++static struct __rpc_msg *msgs_head = NULL;
++static struct __rpc_msg *msgs_tail = NULL;
++
++static void *rpc_malloc(unsigned sz) {
++	 void *ptr = kmalloc(sz, GFP_KERNEL);
++
++	 if(ptr)
++		 return ptr;
++
++	 printk(KERN_ERR "sgh_rpc: kmalloc of %d failed, retrying...\n", sz);
++
++	 do {
++		 ptr = kmalloc(sz, GFP_KERNEL);
++	 } while (!ptr);
++
++	 return ptr;
++}
++
++static void do_read_data(struct work_struct *work) {
++	struct __rpc_msg *msg;
++	int pktlen, rpclen;
++	unsigned char end_flag;
++	char buf[11];
++	unsigned long flags=0;
++
++	msg = (struct __rpc_msg *)rpc_malloc(sizeof(struct __rpc_msg));
++	msg->data = NULL;
++
++	if(smd_read(CH_RPC, buf,11) == 0) {
++		if(buf[0] != 0x7f) {
++			goto cleanup;
++		}
++
++		pktlen = (buf[2]<<8)|(buf[1]);
++		msg->crc = buf[3];
++		rpclen = (buf[5]<<8)|(buf[4]);
++		msg->len = rpclen - 7;
++		msg->index = (buf[7]<<8)|(buf[6]);
++		msg->command = (buf[8]<<8)|(buf[9]);
++		msg->type = buf[10];
++
++		if(msg->len > 0) {
++			msg->data = (char *)rpc_malloc(msg->len);
++			if(smd_read(CH_RPC, msg->data, msg->len) != 0) {
++				goto cleanup;
++			}
++		}
++
++		if(smd_read(CH_RPC, &end_flag,1)!=0 || end_flag!=0x7e){
++			goto cleanup;
++		}
++
++		spin_lock_irqsave(&msgs_lock, flags);
++		if(msgs_tail != NULL)
++			msgs_tail->next = msg;
++		msgs_tail = msg;
++		msgs_tail->next = NULL;
++		if(msgs_head == NULL)
++			msgs_head = msg;
++		spin_unlock_irqrestore(&msgs_lock, flags);
++
++		goto success;
++
++	}
++
++cleanup:
++	if(msg->data)
++		kfree(msg->data);
++
++	kfree(msg);
++
++success:
++	queue_work(workqueue, &work_read);
++}
++
++static int write_index = 0xff00;
++
++static char __crc;
++
++static char calc_crc() {
++	int64_t m;
++	int u, rc;
++
++	m = (__crc+1) * -2130574327; //0x81020409
++	u = m>>32;
++	u += __crc+1;
++	u >>= 6;
++
++	u += ((unsigned)u>>31);
++	u += ((unsigned)u<<7);
++	u = __crc+1 - u;
++
++	rc = __crc;
++	__crc = u;
++
++	return rc;
++}
++
++static int rpc_write(unsigned cmd, unsigned type,void *data, unsigned len) {
++	char *pkt;
++	char *p;
++	int crc;
++	unsigned long flags=0;
++	unsigned n;
++
++	pkt = rpc_malloc(len+11);
++	p = pkt;
++
++	n = len + 10;
++
++	*p++ = 0x7f;
++	*p++ = n & 0xff;
++	*p++ = (n>>8) & 0xff;
++	spin_lock_irqsave(&crc_lock, flags);
++	crc = calc_crc();
++	spin_unlock_irqrestore(&crc_lock, flags);
++	*p++ = crc;
++
++	n = len + 7;
++	*p++ = (n) & 0xff;
++	*p++ = (n>>8) & 0xff;
++	*p++ = write_index++ & 0xff; //index
++	*p++ = 0xff;
++	*p++ = (cmd>>8) & 0xff;
++	*p++ = cmd & 0xff;
++	*p++ = type & 0xff;
++
++	if(len > 0 && data != NULL) {
++		copy_from_user(p, data, len);
++		p += len;
++	}
++
++	*p++ = 0x7e;
++
++	smd_write(CH_RPC, pkt, (unsigned)(p - pkt));
++
++	kfree(pkt);
++
++	return len;
++}
++
++
++static int rpc_ops_open(struct inode *inode, struct file *filp) {
++	int rc;
++
++	rc = nonseekable_open(inode, filp);
++	if (rc < 0)
++		return rc;
++
++	return 0;
++}
++
++static int rpc_ops_release(struct inode *inode, struct file *filp) {
++	return 0;
++}
++
++static ssize_t rpc_ops_read(struct file *filp, char __user *buf,size_t count, loff_t *ppos) {
++	unsigned long flags = 0;
++	struct __rpc_msg *msg = NULL;
++	int len = 0;
++
++
++	msg = msgs_head;
++	if(msg == NULL) return -EIO;
++
++	spin_lock_irqsave(&msgs_lock, flags);
++	msgs_head = msgs_head->next;
++	if(msgs_head == NULL)
++		msgs_tail = NULL;
++	spin_unlock_irqrestore(&msgs_lock, flags);
++
++	if(msg->data != NULL && msg->len > 0) {
++		len = count > msg->len ? msg->len : count;
++		if(copy_to_user(buf, msg->data, len)!=0)
++			len = -EIO;
++	}
++	if(msg->data != NULL)
++		kfree(msg->data);
++	kfree(msg);
++
++	return len;
++}
++
++static ssize_t rpc_ops_write(struct file *filp, const char __user *buf,size_t count, loff_t *ppos) {
++	char h[6];
++	short *sh;
++
++	if(copy_from_user(h, buf, 6))
++		return 0;
++
++	buf += 6;
++
++	sh = (short *)h;
++
++	return rpc_write(sh[0], sh[1], sh[2]>0 ? buf : NULL, sh[2]);
++}
++
++static unsigned int rpc_ops_poll(struct file *filp, struct poll_table_struct *wait) {
++	unsigned mask = 0;
++
++	return mask;
++}
++
++static long rpc_ops_ioctl(struct file *filp, unsigned int cmd,unsigned long arg) {
++	struct __rpc_msg *msg;
++
++	msg = msgs_head;
++	if(msg == NULL)
++		return -EIO;
++
++	return copy_to_user((void *)arg, msg, 20);
++}
++
++static struct file_operations rpc_fops = {
++	.owner	 = THIS_MODULE,
++	.open	 = rpc_ops_open,
++	.release = rpc_ops_release,
++	.read	 = rpc_ops_read,
++	.write	 = rpc_ops_write,
++	//.poll    = rpc_ops_poll,
++	.unlocked_ioctl	 = rpc_ops_ioctl,
++
++};
++
++
++void rpc_init(void) {
++	int rc;
++	int major;
++
++	smd_init();
++
++	/* Create the device nodes */
++	sgh_rpc_class = class_create(THIS_MODULE, "sghrpc");
++	if (IS_ERR(sgh_rpc_class)) {
++		rc = -ENOMEM;
++		printk(KERN_ERR
++			       "sgh_rpc: failed to create sghrpc class\n");
++		return;
++	}
++
++	rc = alloc_chrdev_region(&sgh_rpc_devno, 0, 1, "sghrpc");
++	if (rc < 0) {
++		printk(KERN_ERR
++		       "rpcrouter: Failed to alloc chardev region (%d)\n", rc);
++		goto fail_destroy_class;
++	}
++
++	major = MAJOR(sgh_rpc_devno);
++	rpc_device = device_create(sgh_rpc_class, NULL,
++						 sgh_rpc_devno, NULL, "sghrpc%d:%d",
++						 0, 0);
++	if (IS_ERR(rpc_device)) {
++		rc = -ENOMEM;
++		goto fail_unregister_cdev_region;
++	}
++
++	cdev_init(&rpc_cdev, &rpc_fops);
++	rpc_cdev.owner = THIS_MODULE;
++
++	rc = cdev_add(&rpc_cdev, sgh_rpc_devno, 1);
++	if (rc < 0)
++		goto fail_destroy_device;
++
++	workqueue = create_singlethread_workqueue("sgh-rpc");
++	queue_work(workqueue, &work_read);
++
++	return;
++
++fail_destroy_device:
++	device_destroy(sgh_rpc_class, sgh_rpc_devno);
++fail_unregister_cdev_region:
++	unregister_chrdev_region(sgh_rpc_devno, 1);
++fail_destroy_class:
++	class_destroy(sgh_rpc_class);
++}
+diff -ur linux-2.6.32/arch/arm/mach-pxa/sgh_smd.c kernel/arch/arm/mach-pxa/sgh_smd.c
+--- linux-2.6.32/arch/arm/mach-pxa/sgh_smd.c	2009-12-13 13:01:03.799036125 +0200
++++ kernel/arch/arm/mach-pxa/sgh_smd.c	2009-12-12 16:09:26.486282481 +0200
+@@ -0,0 +1,458 @@
++/**
++ * Support for Samsung SGH I900 MSM6K Shared Memory
++ *
++ * Copyright (C) 2009 Mustafa Ozsakalli <ozsakalli@hotmail.com>
++ *
++ * This program is free software; you can redistribute it and/or modify
++ * it under the terms of the GNU General Public License version 2 as
++ * published by the Free Software Foundation.
++ */
++
++#include <linux/platform_device.h>
++#include <linux/module.h>
++#include <linux/kernel.h>
++#include <linux/init.h>
++#include <linux/interrupt.h>
++#include <linux/irq.h>
++#include <linux/miscdevice.h>
++#include <linux/io.h>
++#include <linux/gpio.h>
++#include <linux/delay.h>
++#include <linux/freezer.h>
++#include <linux/wait.h>
++#include <linux/workqueue.h>
++#include <linux/completion.h>
++
++#include <mach/hardware.h>
++#include <mach/sgh_msm6k.h>
++
++#include "devices.h"
++
++static unsigned mmio;
++static int smd_initialized;
++
++static DEFINE_SPINLOCK(smd_lock);
++
++#define MEMP(x) (void *)(mmio + x)
++#define MEMW(x) *((unsigned short *)(mmio + x))
++#define MEML(x) *((unsigned long *)(mmio + x))
++
++#define HEAD(c) MEMW(c.head)
++#define TAIL(c) MEMW(c.tail)
++#define HEADPTR(c) MEMP(c.base + HEAD(c))
++#define TAILPTR(c) MEMP(c.base + TAIL(c))
++#define SETTAIL(c,t) TAIL(c)=t; TAIL(c)=t; TAIL(c)=t
++#define SETHEAD(c,h) HEAD(c)=h; HEAD(c)=h; HEAD(c)=h
++#define AVAIL(h,t,s) t<=h ? h-t : s - (t-h)
++
++struct smd_half_channel {
++	unsigned head;
++	unsigned tail;
++	unsigned base;
++	unsigned size;
++};
++
++struct smd_channel {
++	struct smd_half_channel send;
++	struct smd_half_channel recv;
++	unsigned head_mask;
++	unsigned tail_mask;
++	unsigned data_mask;
++	unsigned ex_mask;
++	wait_queue_head_t wait_recv;
++	wait_queue_head_t wait_send;
++};
++
++static struct smd_channel smd_channels[] = {
++		//msm6k rpc channel
++		{
++
++		.send = {
++				.head = 0x4,
++				.tail = 0x6,
++				.base = 0x8,
++				.size = 0x3fc,
++		},
++
++		.recv = {
++				.head = 0x1298,
++				.tail = 0x129a,
++				.base = 0x129c,
++				.size = 0x3fc,
++		},
++
++		.head_mask = 0x2,
++		.tail_mask = 0x8,
++		.data_mask = 0x20,
++
++		},
++
++		{
++
++		.send = {
++				.head = 0x404,
++				.tail = 0x406,
++				.base = 0x408,
++				.size = 0xe90,
++		},
++
++		.recv = {
++				.head = 0x1698,
++				.tail = 0x169a,
++				.base = 0x169c,
++				.size = 0x2950,
++		},
++
++		.head_mask = 0x1,
++		.tail_mask = 0x4,
++		.data_mask = 0x10,
++
++		},
++
++};
++
++void smd_phone_power(int on) {
++	if(on){
++		gpio_set_value(0x66,1);
++		gpio_set_value(0x51,1);
++		mdelay(500);
++		gpio_set_value(0x51,0);
++	} else {
++		gpio_set_value(0x66,0);
++		mdelay(500);
++		gpio_set_value(0x66,1);
++
++	}
++}
++
++
++void smd_init_mem(void)
++{
++	int i;
++
++	if(smd_initialized)
++		return;
++
++	MEML(0x20) = 0;
++	MEMW(0x3ffe) = 0x00C1;
++	MEML(0x20) = 0;
++
++	MEMW(0x2) = 0;
++	MEMW(0x4) = 0;
++	MEMW(0x6) = 0;
++
++	for(i = 8; i < 0x404; i += 2)
++		MEMW(i) = 0x1111;
++
++	MEMW(0x404) = 0;
++	MEMW(0x406) = 0;
++
++    for(i = 0x408; i < 0x1298; i += 2)
++    	MEMW(i) = 0x2222;
++
++    MEMW(0x1298) = 0;
++    MEMW(0x129A) = 0;
++
++    for(i = 0x129C; i < 0x1698; i += 2)
++    	MEMW(i) = 0x3333;
++
++    MEMW(0x1698) = 0;
++    MEMW(0x169A) = 0;
++
++    for(i = 0x169C; i < 0x3FEC; i += 2)
++    	MEMW(i) = 0x4444;
++
++    if(MEML(0x18) == 0) {
++    	MEMW(0) = 0x00AA;
++    	MEMW(2) = 0x0001;
++     }
++
++    MEMW(0x3ffe) = 0x00C2;
++	MEML(0x20) = 0;
++
++	smd_initialized = 1;
++
++    printk("SMD: Initialize Completed\n");
++}
++
++static int smd_write_and_check(unsigned adr, void* data, int len) {
++	int try;
++
++	for(try=0; try<3; try++){
++		memcpy(MEMP(adr), data, len);
++		if(memcmp(MEMP(adr), data, len)==0) break;
++	}
++
++	if(adr == 0x3FFE)
++		MEML(0x20) = 0;
++
++	return try<3 ? 1 : 0;
++
++}
++
++static int smd_read_and_check(unsigned adr, void *data, int len) {
++	int try;
++
++
++	for(try=0;try<3;try++){
++		memcpy(data,MEMP(adr),len);
++		if(memcmp(data,MEMP(adr),len)==0) break;
++	}
++
++	if(try > 2 || adr == 0x3ffc)
++		MEML(0x20) = 0;
++
++	//synch problem
++	if(try>2) return 0;
++
++	return 1;
++
++}
++
++static int smd_get_mask() {
++	unsigned short mask;
++
++	smd_read_and_check(0x3ffc, &mask, 2);
++
++	return mask;
++}
++
++static void smd_set_mask(short mask) {
++	smd_write_and_check(0x3ffe, &mask, 2);
++}
++
++irqreturn_t smd_irq_handler(int irq, void *dev_id){
++	unsigned long flags;
++	int mask,i;
++
++	//printk("SMD: IRQ fired\n");
++
++	spin_lock_irqsave(&smd_lock, flags);
++
++	mask = smd_get_mask();
++
++	if(!(mask&0x80)) goto done;
++
++	switch(mask & ~0x80){
++	case 0x48 :	//initialize
++		smd_init_mem();
++		break;
++
++	case 0x4A :
++		printk("SMD: Phone Deep Sleep??\n");
++		/*fire PhoneDeepSleepEvent?*/
++		break;
++	}
++
++
++	for(i=0;i<2;i++){
++		struct smd_channel *c = &smd_channels[i];
++		if(HEAD(c->recv) != TAIL(c->recv))
++			mask |= c->head_mask;
++	}
++
++	if((mask & 0x2a) != 0)
++		smd_channels[0].ex_mask = mask;
++
++	if((mask & 0x15) != 0)
++		smd_channels[1].ex_mask = mask;
++
++
++	for(i=0;i<2;i++){
++		struct smd_channel *c = &smd_channels[i];
++
++		if(HEAD(c->recv) != TAIL(c->recv)){
++			wake_up(&c->wait_recv);
++		}
++
++		if((mask & (0x80 | c->tail_mask)) != 0)
++			wake_up(&c->wait_send);
++	}
++
++done:
++	spin_unlock_irqrestore(&smd_lock, flags);
++
++	return IRQ_HANDLED;
++}
++
++int smd_read_avail(struct smd_channel *c) {
++	unsigned head, tail;
++
++	if(!smd_initialized)
++		return 0;
++
++	head = HEAD(c->recv);
++	tail = TAIL(c->recv);
++
++	return AVAIL(head,tail,c->recv.size);
++}
++
++int ch_read(struct smd_channel *c, void *_buf, int len) {
++	int n;
++	int head, tail;
++	int orig_len = len;
++	unsigned char *buf = _buf;
++
++	if(!smd_initialized) return 0;
++
++
++	while(len > 0) {
++		head = HEAD(c->recv);
++		tail = TAIL(c->recv);
++
++		n = tail<=head ? head - tail : c->recv.size - tail;
++		if(n==0) break;
++
++		if(n > len) n = len;
++
++		memcpy(buf, TAILPTR(c->recv), n);
++
++		buf += n;
++		len -= n;
++
++		tail = (tail + n) % (c->recv.size);
++		SETTAIL(c->recv,tail);
++	}
++
++	if(orig_len!=len || HEAD(c->recv)==TAIL(c->recv)) {
++		int mask = c->ex_mask;
++		mask &= c->data_mask;
++		if(mask != 0)
++			smd_set_mask(0x80 | c->tail_mask);
++	}
++
++	return orig_len - len;
++}
++
++int ch_write(struct smd_channel *c, void *buf, int len) {
++	unsigned head, tail ,mask;
++	int n;
++
++	head = HEAD(c->send);
++	tail = TAIL(c->send);
++
++	n = (head < tail) ? tail - head :
++	    c->send.size - head;
++
++
++	mask = 0x80;
++
++	if(n > len) n = len;
++
++	if(n > 0) {
++		memcpy(HEADPTR(c->send), buf, n);
++		head = (head + n) % c->send.size;
++		SETHEAD(c->send, head);
++		mask |= c->head_mask;
++	}
++	head = HEAD(c->send);
++	tail = TAIL(c->send);
++
++	if(n < len)
++		mask |= c->data_mask;
++
++	smd_set_mask(mask);
++
++	return n;
++}
++
++struct smd_channel *smd_get_channel(int c) {
++	return &smd_channels[c];
++}
++
++int smd_read(int ch, void *buf, int len) {
++	struct smd_channel *c;
++	unsigned long flags;
++	int rc;
++
++	c = smd_get_channel(ch);
++	for(;;) {
++		spin_lock_irqsave(&smd_lock, flags);
++		if(smd_read_avail(c) >= len) {
++			rc = ch_read(c, buf, len);
++			spin_unlock_irqrestore(&smd_lock, flags);
++			if(rc == len)
++				return 0;
++			else
++				return -EIO;
++		}
++
++		spin_unlock_irqrestore(&smd_lock, flags);
++		wait_event(c->wait_recv, smd_read_avail(c) >= len);
++	}
++
++	return 0;
++}
++
++int smd_write(int ch, void *_buf, int len) {
++	struct smd_channel *c;
++	unsigned long flags;
++	int n;
++	char *buf = _buf;
++
++	c = smd_get_channel(ch);
++	while(len > 0) {
++		spin_lock_irqsave(&smd_lock, flags);
++		n = ch_write(c, buf, len);
++		spin_unlock_irqrestore(&smd_lock, flags);
++
++		len -= n;
++		buf += n;
++
++		wait_event(c->wait_send, len <= 0);
++	}
++
++	return 0;
++}
++
++
++void smd_init(void) {
++	struct resource *r;
++	unsigned short *ram;
++	int rc, i;
++
++	gpio_request(0x6b,"dpram");
++	gpio_request(0x46,"dpram");
++	gpio_request(0x6e,"dpram");
++	gpio_request(0x51,"dpram");
++	gpio_request(0x66,"dpram");
++
++	gpio_direction_output(0x6b,0);
++	gpio_direction_output(0x46,0);
++	gpio_direction_output(0x6e,0);
++	gpio_direction_output(0x51,1);
++	gpio_direction_output(0x66,1);
++
++	smd_phone_power(0);
++
++	gpio_set_value(0x46,0);
++	gpio_set_value(0x6b,0);
++
++	r = request_mem_region(0,0x4000,"dpram");
++	if(r==NULL){
++		printk("SMD: Can't get memory region!\n");
++		return;
++	}
++
++	mmio = (unsigned long)ioremap(r->start,r->end-r->start+1);
++
++	for(i=0;i<2;i++) {
++		init_waitqueue_head(&smd_channels[i].wait_recv);
++		init_waitqueue_head(&smd_channels[i].wait_send);
++	}
++
++	ram = ((unsigned short *)(mmio));
++	//check dpram
++	for(i=0;i<0x2000;i++)
++		ram[i] = 0;
++
++	ram[0] = 0xaa;
++	ram[1] = 1;
++
++	rc = request_irq(IRQ_GPIO(0x46), smd_irq_handler,
++			 IRQF_TRIGGER_RISING | IRQF_TRIGGER_FALLING,
++			 "SMD-17", NULL);
++
++	smd_phone_power(0);
++	smd_phone_power(0);
++}
+diff -ur linux-2.6.32/arch/arm/tools/mach-types kernel/arch/arm/tools/mach-types
+--- linux-2.6.32/arch/arm/tools/mach-types	2009-12-03 05:51:21.000000000 +0200
++++ kernel/arch/arm/tools/mach-types	2009-12-12 16:09:26.746279722 +0200
+@@ -2249,7 +2249,7 @@
+ darwin			MACH_DARWIN		DARWIN			2262
+ oratiscomu		MACH_ORATISCOMU		ORATISCOMU		2263
+ rtsbc20			MACH_RTSBC20		RTSBC20			2264
+-sgh_i780		MACH_I780		I780			2265
++sgh_i780		MACH_SGH_I780		SGH_I780		2265
+ gemini324		MACH_GEMINI324		GEMINI324		2266
+ oratislan		MACH_ORATISLAN		ORATISLAN		2267
+ oratisalog		MACH_ORATISALOG		ORATISALOG		2268
+diff -ur linux-2.6.32/drivers/i2c/busses/i2c-pxa.c kernel/drivers/i2c/busses/i2c-pxa.c
+--- linux-2.6.32/drivers/i2c/busses/i2c-pxa.c	2009-12-03 05:51:21.000000000 +0200
++++ kernel/drivers/i2c/busses/i2c-pxa.c	2009-12-12 16:09:31.776280278 +0200
+@@ -1173,7 +1173,7 @@
+ 		.owner	= THIS_MODULE,
+ 		.pm	= I2C_PXA_DEV_PM_OPS,
+ 	},
+-	.id_table	= i2c_pxa_id_table,
++	.id_table	= &i2c_pxa_id_table,
+ };
+ 
+ static int __init i2c_adap_pxa_init(void)
+diff -ur linux-2.6.32/drivers/input/keyboard/pxa27x_keypad.c kernel/drivers/input/keyboard/pxa27x_keypad.c
+--- linux-2.6.32/drivers/input/keyboard/pxa27x_keypad.c	2009-12-03 05:51:21.000000000 +0200
++++ kernel/drivers/input/keyboard/pxa27x_keypad.c	2009-12-12 16:09:32.012943837 +0200
+@@ -32,6 +32,16 @@
+ 
+ #include <mach/hardware.h>
+ #include <mach/pxa27x_keypad.h>
++#if defined(CONFIG_PXA3xx_DVFM)
++#include <linux/notifier.h>
++#include <linux/timer.h>
++#include <mach/dvfm.h>
++#include <mach/pxa3xx_dvfm.h>
++#endif
++#ifdef CONFIG_ANDROID_POWER
++#include <linux/android_power.h>
++static android_suspend_lock_t pxa27x_keypad_suspend_lock; 
++#endif 
+ /*
+  * Keypad Controller registers
+  */
+@@ -98,6 +108,25 @@
+ #define MAX_MATRIX_KEY_NUM	(MAX_MATRIX_KEY_ROWS * MAX_MATRIX_KEY_COLS)
+ #define MAX_KEYPAD_KEYS		(MAX_MATRIX_KEY_NUM + MAX_DIRECT_KEY_NUM)
+ 
++#if defined(CONFIG_PXA3xx_DVFM)
++#define D2_STABLE_JIFFIES               6
++
++static int keyevent_enable = 0;
++static int keypad_notifier_freq(struct notifier_block *nb,
++		unsigned long val, void *data);
++static struct notifier_block notifier_freq_block = {
++	.notifier_call = keypad_notifier_freq,
++};
++
++static struct dvfm_lock dvfm_lock = {
++	.lock		= SPIN_LOCK_UNLOCKED,
++	.dev_idx	= -1,
++	.count		= 0,
++};
++
++static struct timer_list kp_timer;
++#endif
++
+ struct pxa27x_keypad {
+ 	struct pxa27x_keypad_platform_data *pdata;
+ 
+@@ -334,6 +363,8 @@
+ 	struct pxa27x_keypad *keypad = dev_id;
+ 	unsigned long kpc = keypad_readl(KPC);
+ 
++	printk("-- irq handled --\n");
++
+ 	if (kpc & KPC_DI)
+ 		pxa27x_keypad_scan_direct(keypad);
+ 
+@@ -402,6 +433,87 @@
+ 	clk_disable(keypad->clk);
+ }
+ 
++#if defined(CONFIG_PXA3xx_DVFM)
++static void set_dvfm_constraint(void)
++{
++	spin_lock_irqsave(&dvfm_lock.lock, dvfm_lock.flags);
++	if (dvfm_lock.count++ == 0) {
++		/* Disable lowpower mode */
++		dvfm_disable_op_name("D1", dvfm_lock.dev_idx);
++		dvfm_disable_op_name("D2", dvfm_lock.dev_idx);
++		if (cpu_is_pxa935())
++			dvfm_disable_op_name("CG", dvfm_lock.dev_idx);
++	}
++	spin_unlock_irqrestore(&dvfm_lock.lock, dvfm_lock.flags);
++}
++
++static void unset_dvfm_constraint(void)
++{
++	spin_lock_irqsave(&dvfm_lock.lock, dvfm_lock.flags);
++	if (dvfm_lock.count == 0) {
++		printk(KERN_WARNING "Keypad constraint has been removed.\n");
++	} else if (--dvfm_lock.count == 0) {
++		/* Enable lowpower mode */
++		dvfm_enable_op_name("D1", dvfm_lock.dev_idx);
++		dvfm_enable_op_name("D2", dvfm_lock.dev_idx);
++		if (cpu_is_pxa935())
++			dvfm_enable_op_name("CG", dvfm_lock.dev_idx);
++	}
++	spin_unlock_irqrestore(&dvfm_lock.lock, dvfm_lock.flags);
++}
++
++/*
++ * FIXME: Here a timer is used to disable entering D1/D2 for a while.
++ * Because keypad event wakeup system from D1/D2 mode. But keypad device
++ * can't detect the interrupt since it's in standby state.
++ * Keypad device need time to detect it again. So we use a timer here.
++ * D1/D2 idle is determined by idle time. It's better to comine these
++ * timers together.
++ */
++static void keypad_timer_handler(unsigned long data)
++{
++	unset_dvfm_constraint();
++}
++
++extern void get_wakeup_source(pm_wakeup_src_t *);
++
++static int keypad_notifier_freq(struct notifier_block *nb,
++		unsigned long val, void *data)
++{
++	struct dvfm_freqs *freqs = (struct dvfm_freqs *)data;
++	struct op_info *new = NULL;
++	struct dvfm_md_opt *op;
++	pm_wakeup_src_t src;
++
++	if (freqs)
++		new = &freqs->new_info;
++	else
++		return 0;
++
++	op = (struct dvfm_md_opt *)new->op;
++	if (val == DVFM_FREQ_POSTCHANGE) {
++		if ((op->power_mode == POWER_MODE_D1) ||
++				(op->power_mode == POWER_MODE_D2) ||
++				(op->power_mode == POWER_MODE_CG)) {
++			//get_wakeup_source(&src);
++			//if (src.bits.mkey || src.bits.dkey) {
++				/* If keypad event happens and wake system
++				 * from D1/D2. Disable D1/D2 to make keypad
++				 * work for a while.
++				 */
++				kp_timer.expires = jiffies + D2_STABLE_JIFFIES;
++				add_timer(&kp_timer);
++				set_dvfm_constraint();
++			#ifdef CONFIG_ANDROID_POWER
++				android_lock_suspend_auto_expire(&pxa27x_keypad_suspend_lock, D2_STABLE_JIFFIES);
++			#endif
++			//}
++		}
++	}
++	return 0;
++}
++#endif
++
+ #ifdef CONFIG_PM
+ static int pxa27x_keypad_suspend(struct device *dev)
+ {
+@@ -410,8 +522,10 @@
+ 
+ 	clk_disable(keypad->clk);
+ 
+-	if (device_may_wakeup(&pdev->dev))
++	if (device_may_wakeup(&pdev->dev)) {
++		printk("-- keypad wake set %d\n",keypad->irq);
+ 		enable_irq_wake(keypad->irq);
++	}
+ 
+ 	return 0;
+ }
+@@ -495,6 +609,15 @@
+ 		goto failed_free_mem;
+ 	}
+ 
++#if defined(CONFIG_PXA3xx_DVFM)
++	dvfm_register("Keypad", &dvfm_lock.dev_idx);
++	dvfm_register_notifier(&notifier_freq_block,
++			DVFM_FREQUENCY_NOTIFIER);
++	init_timer(&kp_timer);
++	kp_timer.function = keypad_timer_handler;
++	kp_timer.data = 0;
++#endif
++
+ 	keypad->clk = clk_get(&pdev->dev, NULL);
+ 	if (IS_ERR(keypad->clk)) {
+ 		dev_err(&pdev->dev, "failed to get keypad clock\n");
+@@ -596,11 +719,18 @@
+ 
+ static int __init pxa27x_keypad_init(void)
+ {
++#ifdef CONFIG_ANDROID_POWER
++	pxa27x_keypad_suspend_lock.name = "pxa27x_keypad";
++	android_init_suspend_lock(&pxa27x_keypad_suspend_lock);
++#endif 
+ 	return platform_driver_register(&pxa27x_keypad_driver);
+ }
+ 
+ static void __exit pxa27x_keypad_exit(void)
+ {
++#ifdef CONFIG_ANDROID_POWER
++	android_uninit_suspend_lock(&pxa27x_keypad_suspend_lock);
++#endif 
+ 	platform_driver_unregister(&pxa27x_keypad_driver);
+ }
+ 
+diff -ur linux-2.6.32/drivers/mmc/host/pxamci.c kernel/drivers/mmc/host/pxamci.c
+--- linux-2.6.32/drivers/mmc/host/pxamci.c	2009-12-03 05:51:21.000000000 +0200
++++ kernel/drivers/mmc/host/pxamci.c	2009-12-12 16:09:33.709612828 +0200
+@@ -127,9 +127,10 @@
+ 				break;
+ 			udelay(1);
+ 		} while (timeout--);
+-
++		/*
+ 		if (v & STAT_CLK_EN)
+ 			dev_err(mmc_dev(host->mmc), "unable to stop clock\n");
++		*/
+ 	}
+ }
+ 
+diff -ur linux-2.6.32/drivers/net/wireless/libertas/if_spi.c kernel/drivers/net/wireless/libertas/if_spi.c
+--- linux-2.6.32/drivers/net/wireless/libertas/if_spi.c	2009-12-03 05:51:21.000000000 +0200
++++ kernel/drivers/net/wireless/libertas/if_spi.c	2009-12-12 16:09:35.289611714 +0200
+@@ -1020,9 +1020,9 @@
+ 		lbs_pr_err("Unsupported chip_id: 0x%02x\n", card_id);
+ 		return -EAFNOSUPPORT;
+ 	}
+-	snprintf(helper_fw, IF_SPI_FW_NAME_MAX, "libertas/gspi%d_hlp.bin",
++	snprintf(helper_fw, IF_SPI_FW_NAME_MAX, "gspi%d_hlp.bin",
+ 		 chip_id_to_device_name[i].name);
+-	snprintf(main_fw, IF_SPI_FW_NAME_MAX, "libertas/gspi%d.bin",
++	snprintf(main_fw, IF_SPI_FW_NAME_MAX, "gspi%d.bin",
+ 		 chip_id_to_device_name[i].name);
+ 	return 0;
+ }
+diff -ur linux-2.6.32/drivers/power/Kconfig kernel/drivers/power/Kconfig
+--- linux-2.6.32/drivers/power/Kconfig	2009-12-03 05:51:21.000000000 +0200
++++ kernel/drivers/power/Kconfig	2009-12-12 16:09:35.736280931 +0200
+@@ -110,4 +110,10 @@
+ 	help
+ 	 Say Y to include support for NXP PCF50633 Main Battery Charger.
+ 
++config BATTERY_SGH
++	tristate "SGH battery driver"
++	depends on I2C
++	help
++	 Say Y here to enable support for PM6558(I2C) chip used on Samsung I780/I900.
++
+ endif # POWER_SUPPLY
+diff -ur linux-2.6.32/drivers/power/Makefile kernel/drivers/power/Makefile
+--- linux-2.6.32/drivers/power/Makefile	2009-12-03 05:51:21.000000000 +0200
++++ kernel/drivers/power/Makefile	2009-12-12 16:09:35.736280931 +0200
+@@ -29,3 +29,5 @@
+ obj-$(CONFIG_BATTERY_DA9030)	+= da9030_battery.o
+ obj-$(CONFIG_BATTERY_MAX17040)	+= max17040_battery.o
+ obj-$(CONFIG_CHARGER_PCF50633)	+= pcf50633-charger.o
++obj-$(CONFIG_BATTERY_SGH)	+= sgh_battery.o
++
+diff -ur linux-2.6.32/drivers/power/sgh_battery.c kernel/drivers/power/sgh_battery.c
+--- linux-2.6.32/drivers/power/sgh_battery.c	2009-12-13 13:03:35.181931149 +0200
++++ kernel/drivers/power/sgh_battery.c	2009-12-12 16:09:35.746280509 +0200
+@@ -0,0 +1,474 @@
++/*
++ * Samsung I780/I900 battery driver
++ *
++ * Copyright (C) 2009 Sacha Refshauge <xsacha@gmail.com>
++ *
++ * Based on DQ27x00 battery driver:
++ * Copyright (C) 2008 Rodolfo Giometti <giometti@linux.it>
++ * Copyright (C) 2008 Eurotech S.p.A. <info@eurotech.it>
++ *
++ * which was based on a previous work by Copyright (C) 2008 Texas Instruments, Inc.
++ *
++ * This package is free software; you can redistribute it and/or modify
++ * it under the terms of the GNU General Public License version 2 as
++ * published by the Free Software Foundation.
++ *
++ * THIS PACKAGE IS PROVIDED ``AS IS'' AND WITHOUT ANY EXPRESS OR
++ * IMPLIED WARRANTIES, INCLUDING, WITHOUT LIMITATION, THE IMPLIED
++ * WARRANTIES OF MERCHANTIBILITY AND FITNESS FOR A PARTICULAR PURPOSE.
++ *
++ */
++#include <linux/module.h>
++#include <linux/param.h>
++#include <linux/jiffies.h>
++#include <linux/workqueue.h>
++#include <linux/delay.h>
++#include <linux/platform_device.h>
++#include <linux/power_supply.h>
++#include <linux/idr.h>
++#include <linux/i2c.h>
++#include <linux/gpio.h>
++
++#define DRIVER_VERSION			"1.0.1"
++
++#define SGH_CHARGE_GPIO			88
++
++#define SGH_BATT_REG_TEMP		0x25
++#define SGH_BATT_REG_VDIFF		0x23
++#define SGH_BATT_REG_VOLT		0x21
++
++/* Reg Table
++This is what is known of the register banks in PM6558 by
++observation. Assumed that all registers are WORDs, so
++address increases by 2. Also assumed that all registers
++are 12-bit right justified (& 0xFFF).
++
++Register	Task		Value
++0x21		Voltage		The voltage
++0x23		Charging	True if charging, False if not charging
++0x25		Temperature	The temperature (which is used to determine charge)
++0xC2		Shutdown	Write-only regisiter
++
++*/
++
++struct sgh_batt_device_info;
++struct sgh_batt_access_methods {
++	int (*read)(u8 reg, int *rt_value, int b_single,
++		struct sgh_batt_device_info *di);
++};
++struct sgh_batt_device_info {
++	struct device 		*dev;
++	int			id;
++	int			voltage_uV;
++	int			current_uA;
++	int			temp_C;
++	int			charge_rsoc;
++	struct sgh_batt_access_methods	*bus;
++	struct power_supply	bat;
++	struct power_supply	bat_ac;
++	struct power_supply	bat_usb;
++	
++	struct i2c_client	*client;
++
++	struct delayed_work 	work;
++	struct workqueue_struct *wqueue;
++
++	int			voltage;
++	int			voltage_sum;
++	int			voltage_count;
++	int			capacity;
++	int 			charging_status;
++	int			poll_count;
++};
++
++
++static enum power_supply_property sgh_batt_battery_props[] = {
++	POWER_SUPPLY_PROP_STATUS,
++	POWER_SUPPLY_PROP_HEALTH,
++	POWER_SUPPLY_PROP_TECHNOLOGY,
++	POWER_SUPPLY_PROP_PRESENT,
++	POWER_SUPPLY_PROP_BATT_VOL,
++	POWER_SUPPLY_PROP_CAPACITY,
++	POWER_SUPPLY_PROP_BATT_TEMP,
++};
++
++static enum power_supply_property sgh_batt_power_props[] = {
++	POWER_SUPPLY_PROP_ONLINE,
++};
++
++static int sgh_batt_read(u8 reg, int *rt_value, struct sgh_batt_device_info *di)
++{
++	struct i2c_client *client = di->client;
++
++	*rt_value = be16_to_cpu(i2c_smbus_read_word_data(client, reg));
++	*rt_value = *rt_value & 0xFFF;
++
++	return 0;
++}
++
++/*
++ * Return the battery voltage in millivolts
++ *
++ */
++static int sgh_batt_get_voltage(struct sgh_batt_device_info *di)
++{
++	int i;
++	int voltages[5];
++	int voltage = 0, largest = 0, smallest = 0;
++
++	for(i = 0; i < 5; i++)
++	{
++		sgh_batt_read(SGH_BATT_REG_VOLT, &voltages[i], di);
++		if (voltages[i] > voltages[largest])
++			largest = i;
++
++		if (voltages[i] < voltages[smallest])
++			smallest = i;
++	}
++	for(i = 0; i < 5; i++)
++	{
++		if (i != smallest && i != largest)
++			voltage += voltages[i];
++	}
++
++	voltage /= 3;
++
++	if(di->voltage_count < 10) {
++		di->voltage_sum += voltage;
++		di->voltage_count++;
++		voltage = di->voltage_sum / di->voltage_count;
++	} else {
++		di->voltage_sum = di->voltage_sum - di->voltage + voltage;
++		voltage = di->voltage_sum / 10;
++	}
++
++	return voltage;
++}
++
++/*
++ * Return the battery temperature in (10x) Celcius degrees.
++ *
++ * From Windows Mobile:
++ * Temp Sample [ Min: 0x21B, 0x368, 0x89e : Max]
++ *                    539    872    2206
++ */
++static int sgh_batt_get_temp(struct sgh_batt_device_info *di)
++{
++	int temp = 0;
++
++	sgh_batt_read(SGH_BATT_REG_TEMP, &temp, di);
++
++	return temp >> 2;
++}
++
++/*
++ * Return the battery charge in percentage.
++ */
++static int sgh_batt_get_charge(struct sgh_batt_device_info *di)
++{
++	int volt = di->voltage;
++	int i, k = 0, d = 10;
++	int ndist, tdist;
++	int vsamp[] = {0xe38, 0xdb6, 0xd66, 0xd25, 0xce4, 0xc94, 0xb79};
++	// Charging applies a greater voltage. USB: ~0x30 AC: ~0x60
++        // volt -=  be16_to_cpu(i2c_smbus_read_word_data(di->client, SGH_BATT_REG_VDIFF)) & 0xFFF;	// FIXME
++
++	/* Use voltage to work out charge.
++	   Closer to 100%, the voltage has less impact on gradient (linear).
++	   Whereas closer to 0%, it is purely the gradient.
++	 */
++	for (i = 6; i >= 0; i--)
++	{
++		if (volt < vsamp[i])
++		{
++			switch (i) {
++			case 0:
++				k = k + 1;
++			case 1:
++				k = k + 1;
++			case 2:
++				k = k + 1;
++			case 3:
++				d = d >> 1;
++			case 4:
++				k = k + 1;
++			case 5:
++				ndist = 100 * (volt - vsamp[i+1]);
++				tdist = (vsamp[i] - vsamp[i+1]);
++				volt = (k * 100) + (ndist / tdist);
++				return volt / d;
++			default:
++				return 0;
++			}
++		}
++	}
++	return 100;
++}
++
++#define to_sgh_batt_device_info(x) container_of((x), \
++				struct sgh_batt_device_info, bat);
++
++static int sgh_batt_battery_get_property(struct power_supply *psy,
++					enum power_supply_property psp,
++					union power_supply_propval *val)
++{
++	struct sgh_batt_device_info *di = to_sgh_batt_device_info(psy);
++	switch (psp) {
++
++	case POWER_SUPPLY_PROP_BATT_VOL:
++		val->intval = sgh_batt_get_voltage(di);
++		break;
++
++	case POWER_SUPPLY_PROP_PRESENT:
++		val->intval = true; // Device can't run without it
++		break;
++	case POWER_SUPPLY_PROP_CAPACITY:
++		val->intval = di->capacity;
++		break;
++	case POWER_SUPPLY_PROP_BATT_TEMP:
++		val->intval = sgh_batt_get_temp(di);
++		break;
++
++	case POWER_SUPPLY_PROP_STATUS:
++		val->intval = di->charging_status ? POWER_SUPPLY_STATUS_CHARGING : POWER_SUPPLY_STATUS_NOT_CHARGING;
++		break;
++	case POWER_SUPPLY_PROP_TECHNOLOGY:
++		val->intval = POWER_SUPPLY_TECHNOLOGY_LION;
++		break;
++ 	case POWER_SUPPLY_PROP_HEALTH:
++                val->intval = POWER_SUPPLY_HEALTH_GOOD;
++                break;
++	default:
++		return -EINVAL;
++	}
++
++	return 0;
++}
++
++static int sgh_batt_power_get_property(struct power_supply *psy,
++					enum power_supply_property psp,
++					union power_supply_propval *val)
++{
++	struct sgh_batt_device_info *di = to_sgh_batt_device_info(psy);
++	switch (psp) {
++	case POWER_SUPPLY_PROP_ONLINE:
++		if (psy->type == POWER_SUPPLY_TYPE_MAINS)
++			val->intval = (di->charging_status == 1) ? 1 : 0;
++		else if (psy->type == POWER_SUPPLY_TYPE_USB)
++			val->intval = (di->charging_status == 2) ? 1 : 0;
++		else	val->intval = 0;
++		break;
++	default:
++		return -EINVAL;
++	}
++
++	return 0;
++}
++
++static void sgh_batt_battery_update(struct power_supply *psy)
++{
++	int charging_status;
++	struct sgh_batt_device_info *di = to_sgh_batt_device_info(psy);
++	charging_status = di->charging_status;
++
++	di->poll_count++;
++	if(gpio_get_value(SGH_CHARGE_GPIO)) {
++		di->charging_status = 0; //not charging
++	} else {
++		di->charging_status = 1; //ac
++		//TODO: Detect usb
++	}
++
++	if(di->charging_status != charging_status || di->poll_count >= 5) {
++		if(di->charging_status != charging_status)
++			di->voltage_sum = di->voltage_count = 0;
++
++		di->voltage = sgh_batt_get_voltage(di);
++		di->capacity = sgh_batt_get_charge(di);
++
++		printk("pwr: V:%x C:%d\n",di->voltage,di->capacity);
++
++		power_supply_changed(psy);
++		di->poll_count = 0;
++	}
++
++	/*
++	di->charging_status = gpio_get_value(SGH_CHARGE_GPIO) ?
++                        POWER_SUPPLY_STATUS_NOT_CHARGING :
++                        POWER_SUPPLY_STATUS_CHARGING;
++	*/
++/*
++	if (di->charging_status != charging_status)
++	{
++		di->reset_avg = 1;
++		di->poll_count = 0xff;
++	}
++
++	if(di->poll_count >= 10) {
++		di->poll_count = 0;
++		power_supply_changed(psy);
++	}
++*/
++}
++
++static void sgh_batt_battery_work(struct work_struct *work)
++{
++	struct sgh_batt_device_info *di = container_of(work, struct sgh_batt_device_info, work.work);
++
++	sgh_batt_battery_update(&di->bat);
++	queue_delayed_work(di->wqueue, &di->work, HZ*5);
++}
++
++static char *supply_list[] = {
++	"battery",
++};
++ 
++static void sgh_powersupply_init(struct sgh_batt_device_info *di) {
++	di->bat.type = POWER_SUPPLY_TYPE_BATTERY;
++        di->bat.properties = sgh_batt_battery_props;
++        di->bat.num_properties = ARRAY_SIZE(sgh_batt_battery_props);
++        di->bat.get_property = sgh_batt_battery_get_property;
++        di->bat.external_power_changed = NULL;
++}
++
++static void sgh_powersupply_power_init(struct power_supply *bat,int is_usb) {
++	bat->name = is_usb ? "usb" : "ac";
++	bat->type = is_usb ? POWER_SUPPLY_TYPE_USB : POWER_SUPPLY_TYPE_MAINS;
++	bat->supplied_to = supply_list;
++	bat->num_supplicants = ARRAY_SIZE(supply_list);
++	bat->properties = sgh_batt_power_props;
++	bat->num_properties = ARRAY_SIZE(sgh_batt_power_props);
++	bat->get_property = sgh_batt_power_get_property;
++}
++
++static int sgh_batt_battery_probe(struct i2c_client *client,
++				 const struct i2c_device_id *id)
++{
++	struct sgh_batt_device_info *di;
++	struct sgh_batt_access_methods *bus;
++	int retval = 0;
++
++        retval = gpio_request(SGH_CHARGE_GPIO, "BATT CHRG");
++	if (retval)
++		goto batt_failed_0;
++
++	di = kzalloc(sizeof(*di), GFP_KERNEL);
++	if (!di) {
++		dev_err(&client->dev, "failed to allocate device info data\n");
++		retval = -ENOMEM;
++		return retval;
++	}
++
++	bus = kzalloc(sizeof(*bus), GFP_KERNEL);
++	if (!bus) {
++		dev_err(&client->dev, "failed to allocate access method "
++					"data\n");
++		retval = -ENOMEM;
++		goto batt_failed_1;
++	}
++
++	i2c_set_clientdata(client, di);
++	di->dev = &client->dev;
++	di->bat.name = "battery";	// Android only looks for this
++	di->bus = bus;
++	di->client = client;
++	di->poll_count = 0;
++	sgh_powersupply_init(di);
++	retval = power_supply_register(&client->dev, &di->bat);
++	if (retval) {
++		dev_err(&client->dev, "failed to register battery\n");
++		goto batt_failed_2;
++	}
++
++	sgh_powersupply_power_init(&di->bat_ac,0);
++	retval = power_supply_register(&client->dev, &di->bat_ac);
++	if (retval) {
++		dev_err(&client->dev, "failed to register battery (ac)\n");
++		goto batt_failed_2;
++	}
++
++	sgh_powersupply_power_init(&di->bat_usb,1);
++	retval = power_supply_register(&client->dev, &di->bat_usb);
++	if (retval) {
++		dev_err(&client->dev, "failed to register battery (usb)\n");
++		goto batt_failed_2;
++	}
++
++	INIT_DELAYED_WORK(&di->work, sgh_batt_battery_work);
++	di->wqueue = create_singlethread_workqueue("battery");
++	queue_delayed_work(di->wqueue, &di->work, 1);
++
++	dev_info(&client->dev, "support ver. %s enabled\n", DRIVER_VERSION);
++
++	return 0;
++
++batt_failed_2:
++	kfree(bus);
++batt_failed_1:
++	kfree(di);
++batt_failed_0:
++
++	return retval;
++}
++
++static int sgh_batt_battery_remove(struct i2c_client *client)
++{
++	struct sgh_batt_device_info *di = i2c_get_clientdata(client);
++
++	cancel_rearming_delayed_workqueue(di->wqueue,
++					  &di->work);
++	destroy_workqueue(di->wqueue);
++
++	gpio_free(SGH_CHARGE_GPIO);
++
++	power_supply_unregister(&di->bat);
++
++	kfree(di->bat.name);
++
++	kfree(di);
++
++	return 0;
++}
++
++
++/*
++ * Module stuff
++ */
++
++static const struct i2c_device_id sgh_batt_id[] = {
++	{ "sgh_battery", 0 },
++	{},
++};
++
++static struct i2c_driver sgh_batt_battery_driver = {
++	.driver = {
++		.name = "battery",
++	},
++	.probe = sgh_batt_battery_probe,
++	.remove = sgh_batt_battery_remove,
++	.suspend = NULL, 
++	.resume = NULL,   //todo: power management
++	.id_table = sgh_batt_id,
++};
++
++static int __init sgh_batt_battery_init(void)
++{
++	int ret;
++
++	ret = i2c_add_driver(&sgh_batt_battery_driver);
++	if (ret)
++		printk(KERN_ERR "Unable to register Samsung I780/I900 driver\n");
++
++	return ret;
++}
++module_init(sgh_batt_battery_init);
++
++static void __exit sgh_batt_battery_exit(void)
++{
++	i2c_del_driver(&sgh_batt_battery_driver);
++}
++module_exit(sgh_batt_battery_exit);
++
++MODULE_AUTHOR("Sacha Refshauge <xsacha@gmail.com>");
++MODULE_DESCRIPTION("Samsung I780/I900 battery monitor driver");
++MODULE_LICENSE("GPL");
+diff -ur linux-2.6.32/drivers/video/pxafb.c kernel/drivers/video/pxafb.c
+--- linux-2.6.32/drivers/video/pxafb.c	2009-12-03 05:51:21.000000000 +0200
++++ kernel/drivers/video/pxafb.c	2009-12-13 14:40:18.638427304 +0200
+@@ -62,6 +62,11 @@
+ #include <mach/bitfield.h>
+ #include <mach/pxafb.h>
+ 
++#ifdef CONFIG_PXA3xx_DVFM
++#include <mach/dvfm.h>
++#include <mach/pxa3xx_dvfm.h>
++#endif
++
+ /*
+  * Complain if VAR is out of range.
+  */
+@@ -86,6 +91,19 @@
+ 
+ static unsigned long video_mem_size = 0;
+ 
++#ifdef CONFIG_PXA3xx_DVFM
++static int fb_notifier_freq(struct notifier_block *nb,
++		unsigned long val, void *data);
++static struct notifier_block notifier_freq_block = {
++	.notifier_call = fb_notifier_freq,
++};
++
++static void *dev_id = NULL;
++
++static int hss = 0;
++static int pxafb_adjust_pcd(struct pxafb_info *fbi, int hss);
++#endif
++
+ static inline unsigned long
+ lcd_readl(struct pxafb_info *fbi, unsigned int off)
+ {
+@@ -1468,6 +1486,11 @@
+ 		lcd_writel(fbi, LCSR1, lcsr1);
+ 	}
+ #endif
++	if( lcsr & (LCSR_BS|LCSR_SOF))
++	{
++		wake_up(&fbi->ctrlr_wait);
++	}
++
+ 	return IRQ_HANDLED;
+ }
+ 
+@@ -1642,7 +1665,7 @@
+ {
+ 	struct pxafb_info *fbi = dev_get_drvdata(dev);
+ 
+-	set_ctrlr_state(fbi, C_DISABLE_PM);
++	//set_ctrlr_state(fbi, C_DISABLE_PM);
+ 	return 0;
+ }
+ 
+@@ -1650,7 +1673,7 @@
+ {
+ 	struct pxafb_info *fbi = dev_get_drvdata(dev);
+ 
+-	set_ctrlr_state(fbi, C_ENABLE_PM);
++	//set_ctrlr_state(fbi, C_ENABLE_PM);
+ 	return 0;
+ }
+ 
+@@ -1660,6 +1683,87 @@
+ };
+ #endif
+ 
++#ifdef CONFIG_PXA3xx_DVFM
++static int dvfm_dev_idx;
++static void set_dvfm_constraint(void)
++{
++	/* Disable Lowpower mode */
++	/* Remove D0CS constraint since LCCR3_STALL is set */
++//	dvfm_disable_op_name("D0CS", dvfm_dev_idx);
++	dvfm_disable_op_name("D1", dvfm_dev_idx);
++	dvfm_disable_op_name("D2", dvfm_dev_idx);
++	if (cpu_is_pxa935())
++		dvfm_disable_op_name("CG", dvfm_dev_idx);
++}
++
++static void unset_dvfm_constraint(void)
++{
++	/* Enable Lowpower mode */
++	/* Remove D0CS constraint since LCCR3_STALL is set */
++//	dvfm_enable_op_name("D0CS", dvfm_dev_idx);
++	dvfm_enable_op_name("D1", dvfm_dev_idx);
++	dvfm_enable_op_name("D2", dvfm_dev_idx);
++	if (cpu_is_pxa935())
++		dvfm_enable_op_name("CG", dvfm_dev_idx);
++}
++
++static int fb_notifier_freq(struct notifier_block *nb,
++				unsigned long val, void *data)
++{
++	struct dvfm_freqs *freqs = (struct dvfm_freqs *)data;
++	struct op_info *new = NULL;
++	struct dvfm_md_opt *op;
++/*
++	if (freqs) {
++		new = &freqs->new_info;
++	} else
++		return 0;
++
++	op = (struct dvfm_md_opt *)new->op;
++	switch (val) {
++	case DVFM_FREQ_PRECHANGE:
++		if ((op->power_mode == POWER_MODE_D0) ||
++			(op->power_mode == POWER_MODE_D0CS))
++			hss = op->hss;
++		else if ((op->power_mode == POWER_MODE_D1) ||
++			(op->power_mode == POWER_MODE_D2) ||
++			(op->power_mode == POWER_MODE_CG))
++			lcd_update = 0;
++		break;
++	case DVFM_FREQ_POSTCHANGE:
++		if ((op->power_mode == POWER_MODE_D1) ||
++			(op->power_mode == POWER_MODE_D2) ||
++			(op->power_mode == POWER_MODE_CG))
++			lcd_update = 1;
++		break;
++	}
++*/
++	return 0;
++}
++
++static int pxafb_adjust_pcd(struct pxafb_info *fbi, int hss)
++{
++
++	return 0;
++}
++
++void pxafb_set_pcd(void)
++{
++/*
++	struct pxafb_info *fbi = (struct pxafb_info *)dev_id;
++
++	if (fbi)
++		pxafb_adjust_pcd(fbi, hss);
++*/
++	return;
++}
++
++EXPORT_SYMBOL(pxafb_set_pcd);
++#else
++static void set_dvfm_constraint(void) {}
++static void unset_dvfm_constraint(void) {}
++#endif
++
+ static int __devinit pxafb_init_video_memory(struct pxafb_info *fbi)
+ {
+ 	int size = PAGE_ALIGN(fbi->video_mem_size);
+
+diff -ur linux-2.6.32/include/linux/input.h kernel/include/linux/input.h
+--- linux-2.6.32/include/linux/input.h	2009-12-03 05:51:21.000000000 +0200
++++ kernel/include/linux/input.h	2009-12-12 16:09:40.056274324 +0200
+@@ -333,6 +333,7 @@
+ #define KEY_BASSBOOST		209
+ #define KEY_PRINT		210	/* AC Print */
+ #define KEY_HP			211
++#define KEY_CAMERAFOCUS		211
+ #define KEY_CAMERA		212
+ #define KEY_SOUND		213
+ #define KEY_QUESTION		214
+diff -ur linux-2.6.32/include/linux/power_supply.h kernel/include/linux/power_supply.h
+--- linux-2.6.32/include/linux/power_supply.h	2009-12-03 05:51:21.000000000 +0200
++++ kernel/include/linux/power_supply.h	2009-12-12 16:09:40.166275516 +0200
+@@ -113,6 +113,8 @@
+ 	POWER_SUPPLY_PROP_TIME_TO_EMPTY_AVG,
+ 	POWER_SUPPLY_PROP_TIME_TO_FULL_NOW,
+ 	POWER_SUPPLY_PROP_TIME_TO_FULL_AVG,
++	POWER_SUPPLY_PROP_BATT_VOL,
++	POWER_SUPPLY_PROP_BATT_TEMP,
+ 	/* Properties of type `const char *' */
+ 	POWER_SUPPLY_PROP_MODEL_NAME,
+ 	POWER_SUPPLY_PROP_MANUFACTURER,
+diff -ur linux-2.6.32/include/linux/time.h kernel/include/linux/time.h
+--- linux-2.6.32/include/linux/time.h	2009-12-03 05:51:21.000000000 +0200
++++ kernel/include/linux/time.h	2009-12-12 16:09:40.246280545 +0200
+@@ -107,6 +107,7 @@
+ extern int no_sync_cmos_clock __read_mostly;
+ void timekeeping_init(void);
+ extern int timekeeping_suspended;
++extern void update_sleep_time(struct timespec ts); 
+ 
+ unsigned long get_seconds(void);
+ struct timespec current_kernel_time(void);
+diff -ur linux-2.6.32/kernel/printk.c kernel/kernel/printk.c
+--- linux-2.6.32/kernel/printk.c	2009-12-03 05:51:21.000000000 +0200
++++ kernel/kernel/printk.c	2009-12-12 16:09:40.512534939 +0200
+@@ -257,6 +257,53 @@
+ #endif
+ 
+ /*
++ * Return the number of unread characters in the log buffer.
++ */
++static int log_buf_get_len(void)
++{
++	return logged_chars;
++}
++
++/*
++ * Clears the ring-buffer
++ */
++void log_buf_clear(void)
++{
++	logged_chars = 0;
++}
++
++/*
++ * Copy a range of characters from the log buffer.
++ */
++int log_buf_copy(char *dest, int idx, int len)
++{
++	int ret, max;
++	bool took_lock = false;
++
++	if (!oops_in_progress) {
++		spin_lock_irq(&logbuf_lock);
++		took_lock = true;
++	}
++
++	max = log_buf_get_len();
++	if (idx < 0 || idx >= max) {
++		ret = -1;
++	} else {
++		if (len > max - idx)
++			len = max - idx;
++		ret = len;
++		idx += (log_end - max);
++		while (len-- > 0)
++			dest[len] = LOG_BUF(idx + len);
++	}
++
++	if (took_lock)
++		spin_unlock_irq(&logbuf_lock);
++
++	return ret;
++}
++
++/*
+  * Commands to do_syslog:
+  *
+  * 	0 -- Close the log.  Currently a NOP.
+@@ -1405,3 +1452,4 @@
+ }
+ EXPORT_SYMBOL(printk_timed_ratelimit);
+ #endif
++
+diff -ur linux-2.6.32/kernel/time/timekeeping.c kernel/kernel/time/timekeeping.c
+--- linux-2.6.32/kernel/time/timekeeping.c	2009-12-03 05:51:21.000000000 +0200
++++ kernel/kernel/time/timekeeping.c	2009-12-12 16:09:40.559199813 +0200
+@@ -886,3 +886,14 @@
+ 				now.tv_nsec + mono.tv_nsec);
+ 	return now;
+ }
++
++void update_sleep_time(struct timespec ts)
++{
++	long wtm_sec, wtm_nsec;
++	wtm_sec  = wall_to_monotonic.tv_sec - ts.tv_sec;
++	wtm_nsec = wall_to_monotonic.tv_nsec - ts.tv_nsec;
++	set_normalized_timespec(&wall_to_monotonic, wtm_sec, wtm_nsec);
++	set_normalized_timespec(&total_sleep_time,
++		(ts.tv_sec + total_sleep_time.tv_sec),
++		(ts.tv_nsec + total_sleep_time.tv_nsec));
++} 
+diff -ur linux-2.6.32/sound/soc/pxa/Kconfig kernel/sound/soc/pxa/Kconfig
+--- linux-2.6.32/sound/soc/pxa/Kconfig	2009-12-03 05:51:21.000000000 +0200
++++ kernel/sound/soc/pxa/Kconfig	2009-12-12 16:09:41.692528097 +0200
+@@ -144,3 +144,14 @@
+        help
+          Say Y if you want to add support for SoC audio on the
+ 	 IMote 2.
++
++config SND_SOC_SGH
++	tristate "SoC Audio support for Samsung SGH I900"
++	depends on SND_PXA2XX_SOC && MACH_SGH_I900
++	select SND_PXA2XX_SOC_AC97
++	select SND_PXA_SOC_SSP
++	select SND_SOC_WM9713
++	help
++	  Say Y if you want to add support for SoC audio on the
++	  Samsung SGH I900 mobile phone.
++
+diff -ur linux-2.6.32/sound/soc/pxa/Makefile kernel/sound/soc/pxa/Makefile
+--- linux-2.6.32/sound/soc/pxa/Makefile	2009-12-03 05:51:21.000000000 +0200
++++ kernel/sound/soc/pxa/Makefile	2009-12-12 16:09:41.692528097 +0200
+@@ -23,6 +23,7 @@
+ snd-soc-magician-objs := magician.o
+ snd-soc-mioa701-objs := mioa701_wm9713.o
+ snd-soc-imote2-objs := imote2.o
++snd-soc-sgh-objs := sgh.o
+ 
+ obj-$(CONFIG_SND_PXA2XX_SOC_CORGI) += snd-soc-corgi.o
+ obj-$(CONFIG_SND_PXA2XX_SOC_POODLE) += snd-soc-poodle.o
+@@ -37,3 +38,4 @@
+ obj-$(CONFIG_SND_PXA2XX_SOC_MIOA701) += snd-soc-mioa701.o
+ obj-$(CONFIG_SND_SOC_ZYLONITE) += snd-soc-zylonite.o
+ obj-$(CONFIG_SND_PXA2XX_SOC_IMOTE2) += snd-soc-imote2.o
++obj-$(CONFIG_SND_SOC_SGH) += snd-soc-sgh.o
+diff -ur linux-2.6.32/sound/soc/pxa/sgh.c kernel/sound/soc/pxa/sgh.c
+--- linux-2.6.32/sound/soc/pxa/sgh.c	2009-12-13 13:07:09.965238502 +0200
++++ kernel/sound/soc/pxa/sgh.c	2009-12-12 16:09:41.695861483 +0200
+@@ -0,0 +1,310 @@
++/*
++ * Handles the Samsung I780-I900 SoC system
++ *
++ * Copyright (C) 2009 Mustafa Ozsakalli
++ *
++ * 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 in version 2 of the License.
++ *
++ * 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., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
++ */
++
++#include <linux/module.h>
++#include <linux/moduleparam.h>
++#include <linux/platform_device.h>
++#include <linux/gpio.h>
++#include <linux/delay.h>
++#include <linux/irq.h>
++
++#include <asm/mach-types.h>
++#include <mach/audio.h>
++
++#include <sound/core.h>
++#include <sound/pcm.h>
++#include <sound/pcm_params.h>
++#include <sound/soc.h>
++#include <sound/soc-dapm.h>
++#include <sound/initval.h>
++#include <sound/ac97_codec.h>
++
++#include "pxa2xx-pcm.h"
++#include "pxa2xx-ac97.h"
++#include "../codecs/wm9713.h"
++#include "pxa-ssp.h"
++
++#define ARRAY_AND_SIZE(x)	(x), ARRAY_SIZE(x)
++
++#define SGH_I780_AUDIO_GPIO	0x13
++#define SGH_I900_AUDIO_GPIO	0x11
++
++static const struct snd_soc_dapm_widget sgh_dapm_widgets[] = {
++	SND_SOC_DAPM_SPK("Front Speaker", NULL),
++	SND_SOC_DAPM_HP("Headset", NULL),
++	SND_SOC_DAPM_LINE("GSM Line Out", NULL),
++	SND_SOC_DAPM_LINE("GSM Line In", NULL),
++	SND_SOC_DAPM_LINE("Radio Line Out", NULL),
++	SND_SOC_DAPM_MIC("Front Mic", NULL),
++};
++
++static const struct snd_soc_dapm_route audio_map[] = {
++	/* Microphone */
++	{"MIC1", NULL, "Front Mic"},
++
++	/* Speaker */
++	{"Front Speaker", NULL, "SPKL"},
++	{"Front Speaker", NULL, "SPKR"},
++
++	/* Earpiece */
++	{"Headset", NULL, "HPL"},
++	{"Headset", NULL, "HPR"},
++
++	/* GSM Module */
++	{"MONOIN", NULL, "GSM Line Out"},
++	{"PCBEEP", NULL, "GSM Line Out"},
++	{"GSM Line In", NULL, "MONO"},
++
++	/* FM Radio Module */
++	{"LINEL", NULL, "Radio Line Out"},
++	{"LINER", NULL, "Radio Line Out"},
++};
++
++static int sgh_wm9713_init(struct snd_soc_codec *codec)
++{
++	unsigned short reg;
++
++	snd_soc_dapm_new_controls(codec, ARRAY_AND_SIZE(sgh_dapm_widgets));
++	snd_soc_dapm_add_routes(codec, ARRAY_AND_SIZE(audio_map));
++
++	snd_soc_dapm_enable_pin(codec, "Front Speaker");
++
++	snd_soc_dapm_sync(codec);
++
++	
++	return 0;
++}
++
++static int sgh_hifi_startup(struct snd_pcm_substream *substream){
++	struct snd_soc_pcm_runtime *rtd = substream->private_data;
++	struct snd_soc_dai *cpu_dai = rtd->dai->cpu_dai;
++
++	cpu_dai->playback.channels_min = 2;
++    cpu_dai->playback.channels_max = 2;
++	
++    return 0;
++}
++
++static int sgh_hifi_prepare(struct snd_pcm_substream *substream) {
++	struct snd_soc_pcm_runtime *rtd = substream->private_data;
++	struct snd_soc_codec *codec = rtd->socdev->card->codec;
++	struct snd_soc_dai *codec_dai = rtd->dai->codec_dai;
++	u16 reg;
++	int gpio = machine_is_sgh_i780() ? SGH_I780_AUDIO_GPIO : SGH_I900_AUDIO_GPIO;
++
++	codec->write(codec, AC97_POWERDOWN, 0);
++	mdelay(1);
++	codec_dai->ops->set_pll(codec_dai, 0, 4096000, 0);
++	schedule_timeout_interruptible(msecs_to_jiffies(10));
++	codec->write(codec, AC97_HANDSET_RATE, 0x0000);
++	schedule_timeout_interruptible(msecs_to_jiffies(10));
++
++	if (substream->stream == SNDRV_PCM_STREAM_PLAYBACK)
++		reg = AC97_PCM_FRONT_DAC_RATE;
++	else
++		reg = AC97_PCM_LR_ADC_RATE;
++	codec->write(codec, AC97_EXTENDED_STATUS, 0x1);
++	codec->write(codec, reg, substream->runtime->rate);
++
++	//Turn on external speaker
++	//TODO: Headset detection
++	gpio_set_value(gpio, 1);
++
++	return 0;
++}
++
++static void sgh_hifi_shutdown(struct snd_pcm_substream *substream) {
++	int gpio = machine_is_sgh_i780() ? SGH_I780_AUDIO_GPIO : SGH_I900_AUDIO_GPIO;
++	gpio_direction_output(gpio, 1);
++	gpio_set_value(gpio, 0);
++}
++
++static int sgh_voice_startup(struct snd_pcm_substream *substream)
++{
++	struct snd_soc_pcm_runtime *rtd = substream->private_data;
++	struct snd_soc_dai *cpu_dai = rtd->dai->cpu_dai;
++
++	cpu_dai->playback.channels_min = 1;
++	cpu_dai->playback.channels_max = 1;
++
++	return 0;
++};
++
++static int sgh_voice_prepare(struct snd_pcm_substream *substream)
++{
++#define WM9713_DR_8000     0x1F40  /*  8000 samples/sec */
++#define WM9713_DR_11025    0x2B11  /* 11025 samples/sec */
++#define WM9713_DR_12000    0x2EE0  /* 12000 samples/sec */
++#define WM9713_DR_16000    0x3E80  /* 16000 samples/sec */
++#define WM9713_DR_22050    0x5622  /* 22050 samples/sec */
++#define WM9713_DR_24000    0x5DC0  /* 24000 samples/sec */
++#define WM9713_DR_32000    0x7D00  /* 32000 samples/sec */
++#define WM9713_DR_44100    0xAC44  /* 44100 samples/sec */
++#define WM9713_DR_48000    0xBB80  /* 48000 samples/sec */
++
++	return 0;
++};
++
++static void sgh_voice_shutdown(struct snd_pcm_substream *substream)
++{
++
++};
++
++static struct snd_soc_ops sgh_ops[] = {
++{
++		.startup	= sgh_hifi_startup,
++		.prepare	= sgh_hifi_prepare,
++		.shutdown 	= sgh_hifi_shutdown,
++},
++{
++		.startup	= sgh_voice_startup,
++		.prepare	= sgh_voice_prepare,
++		.shutdown	= sgh_voice_shutdown,
++},
++};
++
++static struct snd_soc_dai_link sgh_dai[] = {
++	{
++		.name = "AC97",
++		.stream_name = "AC97 HiFi",
++		.cpu_dai = &pxa_ac97_dai[PXA2XX_DAI_AC97_HIFI],
++		.codec_dai = &wm9713_dai[WM9713_DAI_AC97_HIFI],
++		.init = sgh_wm9713_init,
++		.ops = &sgh_ops[0],
++	},
++	{
++		.name = "AC97 Aux",
++		.stream_name = "AC97 Aux",
++		.cpu_dai = &pxa_ac97_dai[PXA2XX_DAI_AC97_AUX],
++		.codec_dai = &wm9713_dai[WM9713_DAI_AC97_AUX],
++	},
++	{
++		.name = "WM9713 Voice",
++		.stream_name = "WM9713 Voice",
++		.cpu_dai = &pxa_ssp_dai[PXA_DAI_SSP3],
++		.codec_dai = &wm9713_dai[WM9713_DAI_PCM_VOICE],
++		.ops = &sgh_ops[1],
++	},
++};
++
++static struct snd_soc_card sgh = {
++	.name = "SGHAudio",
++	.platform = &pxa2xx_soc_platform,
++	.dai_link = sgh_dai,
++	.num_links = ARRAY_SIZE(sgh_dai),
++};
++
++static struct snd_soc_device sgh_snd_devdata = {
++	.card = &sgh,
++	.codec_dev = &soc_codec_dev_wm9713,
++};
++
++static struct platform_device *sgh_snd_device;
++
++static int sgh_wm9713_probe(struct platform_device *pdev)
++{
++	int ret;
++	int gpio = machine_is_sgh_i780() ? SGH_I780_AUDIO_GPIO : SGH_I900_AUDIO_GPIO;
++
++	gpio_request(0x64, "WM9713 Power");
++	gpio_direction_output(0x64, 1);
++	gpio_set_value(0x64, 0);
++	mdelay(10);
++	gpio_set_value(0x64, 1);
++
++	gpio_request(gpio, "Speaker");
++	gpio_direction_output(gpio, 1);
++	gpio_set_value(gpio, 0);
++
++	sgh_snd_device = platform_device_alloc("soc-audio", -1);
++	if (!sgh_snd_device)
++		return -ENOMEM;
++
++	platform_set_drvdata(sgh_snd_device, &sgh_snd_devdata);
++	sgh_snd_devdata.dev = &sgh_snd_device->dev;
++
++	ret = platform_device_add(sgh_snd_device);
++	if (ret != 0)
++		platform_device_put(sgh_snd_device);
++
++	return ret;
++}
++
++static int __devexit sgh_wm9713_remove(struct platform_device *pdev)
++{
++	platform_device_unregister(sgh_snd_device);
++	return 0;
++}
++
++#ifdef CONFIG_PM
++
++static int sgh_wm9713_suspend(struct platform_device *pdev,
++	pm_message_t state)
++{
++	//struct snd_soc_card *card = platform_get_drvdata(pdev);
++	return 0;
++	//return snd_soc_card_suspend_pcms(card, state);
++}
++
++static int sgh_wm9713_resume(struct platform_device *pdev)
++{
++	//struct snd_soc_card *card = platform_get_drvdata(pdev);
++	return 0;
++	//return snd_soc_card_resume_pcms(card);
++}
++
++#else
++#define sgh_wm9713_suspend NULL
++#define sgh_wm9713_resume  NULL
++#define sgh_wm9713_suspend_late NULL
++#define sgh_wm9713_resume_early  NULL
++#endif
++
++static struct platform_driver sgh_wm9713_driver = {
++	.probe			= sgh_wm9713_probe,
++	.remove			= __devexit_p(sgh_wm9713_remove),
++	.suspend		= sgh_wm9713_suspend,
++	.resume			= sgh_wm9713_resume,
++	.driver			= {
++		.name		= "sgh-asoc",
++		.owner		= THIS_MODULE,
++	},
++};
++
++static int __init sgh_asoc_init(void)
++{
++	int ret;
++
++	ret = platform_driver_register(&sgh_wm9713_driver);
++
++	return ret;
++}
++
++static void __exit sgh_asoc_exit(void)
++{
++	platform_driver_unregister(&sgh_wm9713_driver);
++}
++
++module_init(sgh_asoc_init);
++module_exit(sgh_asoc_exit);
++
++/* Module information */
++MODULE_AUTHOR("Mustafa Ozsakalli (ozsakalli@hotmail.com)");
++MODULE_DESCRIPTION("ALSA SoC WM9713 Samsung SGH I780/I900");
++MODULE_LICENSE("GPL");