source and header files added from another kernel source (not available in present repository)

11 files changed, 5854 insertions, 0 deletions

diff --git a/arch/arm/mach-wmt/wmt_clk.c b/arch/arm/mach-wmt/wmt_clk.c
new file mode 100644
index 00000000..c7d5a322
--- /dev/null
+++ b/arch/arm/mach-wmt/wmt_clk.c
@@ -0,0 +1,2378 @@
+/*++
+linux/arch/arm/mach-wmt/wmt_clk.c
+
+Copyright (c) 2008  WonderMedia Technologies, Inc.
+
+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, see <http://www.gnu.org/licenses/>.
+
+WonderMedia Technologies, Inc.
+10F, 529, Chung-Cheng Road, Hsin-Tien, Taipei 231, R.O.C.
+--*/
+
+#include <linux/mtd/mtd.h>
+#include "wmt_clk.h"
+#include <mach/hardware.h>
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/delay.h>
+#include <linux/pm.h>
+#include <linux/cpufreq.h>
+#include <linux/ioport.h>
+#include <linux/platform_device.h>
+
+#include <asm/system.h>
+#include <asm/pgtable.h>
+#include <asm/mach/map.h>
+#include <asm/irq.h>
+#include <asm/sizes.h>
+#include <linux/i2c.h>
+#include <asm/div64.h>
+#include <linux/mutex.h>
+#include <linux/regulator/consumer.h>
+#include <linux/interrupt.h>
+
+#include "generic.h"
+
+#define MMFREQ_VD	DEV_CNMVDU	// DEV_MSVD
+int mmfreq_num = ~0;
+int mmfreq_cur_num = ~0;
+int mmfreq_type_mask;
+int mmfreq_debug;
+struct regulator *re;
+
+#define PMC_BASE PM_CTRL_BASE_ADDR
+#define PMC_PLL (PM_CTRL_BASE_ADDR + 0x200)
+#define PMC_CLK (PM_CTRL_BASE_ADDR + 0x250)
+#define WM8980_SCC_BASE 0xD8110110
+#define CPINFO (WM8980_SCC_BASE + WMT_MMAP_OFFSET)
+
+#define PLL_BUSY 0x7F0038
+#define MAX_DF ((1<<7) - 1)
+#define MAX_DR 1 //((1<<5) - 1)
+#define MAX_DQ ((1<<2) - 1)
+#define GET_DIVF(d) ((((d>>16)&MAX_DF)+1)*2)
+#define GET_DIVR(d) (((d>>8)&MAX_DR)+1)
+#define GET_DIVQ(d) (1<<(d&MAX_DQ))
+
+#define SRC_FREQ25 25
+#define SRC_FREQ24 24
+#define SRC_FREQ SRC_FREQ24
+/*#define debug_clk*/
+extern int wmt_getsyspara(char *varname, char *varval, int *varlen);
+#define DEBUG_PLLA
+/*
+#define DEBUG
+*/
+#ifdef DEBUG
+static int dbg_mask = 1;
+module_param(dbg_mask, int, S_IRUGO | S_IWUSR);
+#define fq_dbg(fmt, args...) \
+	do {\
+		if (dbg_mask) \
+			printk(KERN_ERR "[%s]_%d_%d: " fmt, __func__ , __LINE__, smp_processor_id(), ## args);\
+	} while (0)
+#define fq_trace() \
+	do {\
+		if (dbg_mask) \
+			printk(KERN_ERR "trace in %s %d\n", __func__, __LINE__);\
+	} while (0)
+#else
+#define fq_dbg(fmt, args...)
+#define fq_trace()
+#endif
+extern int wmt_getsyspara(char *varname, char *varval, int *varlen);
+static int dev_en_count[128] = {0};
+struct pll_map pllmap[] = {//total 168
+{126,		0x00290103},{129,		0x002A0103},{132,		0x002B0103},{135,		0x002C0103},{138,		0x002D0103},
+{141,		0x002E0103},{144,		0x002F0103},{147,		0x00300103},{150,		0x00310103},{153,		0x00320103},
+{156,		0x00330103},{159,		0x00340103},{162,		0x00350103},{165,		0x00360103},{168,		0x00370103},
+{171,		0x00380103},{174,		0x00390103},{177,		0x003A0103},{180,		0x003B0103},{183,		0x003C0103},
+{186,		0x003D0103},{189,		0x003E0103},{192,		0x003F0103},{195,		0x00400103},{198,		0x00410103},
+{201,		0x00420103},{204,		0x00430103},{207,		0x00440103},{210,		0x00450103},{213,		0x00460103},
+{216,		0x00470103},{219,		0x00480103},{222,		0x00490103},{225,		0x004A0103},{228,		0x004B0103},
+{231,		0x004C0103},{234,		0x004D0103},{237,		0x004E0103},{240,		0x004F0103},{243,		0x00500103},
+{246,		0x00510103},{249,		0x00520103},{252, 	0x00290102},{258, 	0x002A0102},{264, 	0x002B0102},
+{270, 	0x002C0102},{276, 	0x002D0102},{282, 	0x002E0102},{288, 	0x002F0102},{294, 	0x00300102},
+{300, 	0x00310102},{306, 	0x00320102},{312, 	0x00330102},{318, 	0x00340102},{324, 	0x00350102},
+{330, 	0x00360102},{336, 	0x00370102},{342, 	0x00380102},{348, 	0x00390102},{354, 	0x003A0102},
+{360, 	0x003B0102},{366, 	0x003C0102},{372, 	0x003D0102},{378, 	0x003E0102},{384, 	0x003F0102},
+{390, 	0x00400102},{396, 	0x00410102},{402, 	0x00420102},{408, 	0x00430102},{414, 	0x00440102},
+{420, 	0x00450102},{426, 	0x00460102},{432, 	0x00470102},{438, 	0x00480102},{444, 	0x00490102},
+{450, 	0x004A0102},{456, 	0x004B0102},{462, 	0x004C0102},{468, 	0x004D0102},{474, 	0x004E0102},
+{480, 	0x004F0102},{486, 	0x00500102},{492, 	0x00510102},{498, 	0x00520102},{504, 	0x00290101},
+{516, 	0x002A0101},{528, 	0x002B0101},{540, 	0x002C0101},{552, 	0x002D0101},{564, 	0x002E0101},
+{576, 	0x002F0101},{588, 	0x00300101},{600, 	0x00310101},{612, 	0x00320101},{624, 	0x00330101},
+{636, 	0x00340101},{648, 	0x00350101},{660, 	0x00360101},{672, 	0x00370101},{684, 	0x00380101},
+{696, 	0x00390101},{708, 	0x003A0101},{720, 	0x003B0101},{732, 	0x003C0101},{744, 	0x003D0101},
+{756, 	0x003E0101},{768, 	0x003F0101},{780, 	0x00400101},{792, 	0x00410101},{804, 	0x00420101},
+{816, 	0x00430101},{828, 	0x00440101},{840, 	0x00450101},{852, 	0x00460101},{864, 	0x00470101},
+{876, 	0x00480101},{888, 	0x00490101},{900, 	0x004A0101},{912, 	0x004B0101},{924, 	0x004C0101},
+{936, 	0x004D0101},{948, 	0x004E0101},{960, 	0x004F0101},{972, 	0x00500101},{984, 	0x00510101},
+{996, 	0x00520101},{1008,	0x00290100},{1032,	0x002A0100},{1056,	0x002B0100},{1080,	0x002C0100},
+{1104,	0x002D0100},{1128,	0x002E0100},{1152,	0x002F0100},{1176,	0x00300100},{1200,	0x00310100},
+{1224,	0x00320100},{1248,	0x00330100},{1272,	0x00340100},{1296,	0x00350100},{1320,	0x00360100},
+{1344,	0x00370100},{1368,	0x00380100},{1392,	0x00390100},{1416,	0x003A0100},{1440,	0x003B0100},
+{1464,	0x003C0100},{1488,	0x003D0100},{1512,	0x003E0100},{1536,	0x003F0100},{1560,	0x00400100},
+{1584,	0x00410100},{1608,	0x00420100},{1632,	0x00430100},{1656,	0x00440100},{1680,	0x00450100},
+{1704,	0x00460100},{1728,	0x00470100},{1752,	0x00480100},{1776,	0x00490100},{1800,	0x004A0100},
+{1824,	0x004B0100},{1848,	0x004C0100},{1872,	0x004D0100},{1896,	0x004E0100},{1920,	0x004F0100},
+{1944,	0x00500100},{1968,	0x00510100},{1992,	0x00520100}//,2016,	0x00530100
+};
+
+//total 43+88+88+172 = 391
+struct pll_map pllmapAll[] = {
+{1008,	0x00290100},{504, 	0x00290101},{252, 	0x00290102},{126,	0x00290103},
+{1032,	0x002A0100},{516, 	0x002A0101},{258, 	0x002A0102},{129,	0x002A0103},
+{1056,	0x002B0100},{528, 	0x002B0101},{264, 	0x002B0102},{132,	0x002B0103},
+{1080,	0x002C0100},{540, 	0x002C0101},{270, 	0x002C0102},{135,	0x002C0103},
+{1104,	0x002D0100},{552, 	0x002D0101},{276, 	0x002D0102},{138,	0x002D0103},
+{1128,	0x002E0100},{564, 	0x002E0101},{282, 	0x002E0102},{141,	0x002E0103},
+{1152,	0x002F0100},{576, 	0x002F0101},{288, 	0x002F0102},{144,	0x002F0103},
+{1176,	0x00300100},{588, 	0x00300101},{294, 	0x00300102},{147,	0x00300103},
+{1200,	0x00310100},{600, 	0x00310101},{300, 	0x00310102},{150,	0x00310103},
+{1224,	0x00320100},{612, 	0x00320101},{306, 	0x00320102},{153,	0x00320103},
+{1248,	0x00330100},{624, 	0x00330101},{312, 	0x00330102},{156,	0x00330103},
+{1272,	0x00340100},{636, 	0x00340101},{318, 	0x00340102},{159,	0x00340103},
+{1296,	0x00350100},{648, 	0x00350101},{324, 	0x00350102},{162,	0x00350103},
+{1320,	0x00360100},{660, 	0x00360101},{330, 	0x00360102},{165,	0x00360103},
+{1344,	0x00370100},{672, 	0x00370101},{336, 	0x00370102},{168,	0x00370103},
+{1368,	0x00380100},{684, 	0x00380101},{342, 	0x00380102},{171,	0x00380103},
+{1392,	0x00390100},{696, 	0x00390101},{348, 	0x00390102},{174,	0x00390103},
+{1416,	0x003A0100},{708, 	0x003A0101},{354, 	0x003A0102},{177,	0x003A0103},
+{1440,	0x003B0100},{720, 	0x003B0101},{360, 	0x003B0102},{180,	0x003B0103},
+{1464,	0x003C0100},{732, 	0x003C0101},{366, 	0x003C0102},{183,	0x003C0103},
+{1488,	0x003D0100},{744, 	0x003D0101},{372, 	0x003D0102},{186,	0x003D0103},
+{1512,	0x003E0100},{756, 	0x003E0101},{378, 	0x003E0102},{189,	0x003E0103},
+{1536,	0x003F0100},{768, 	0x003F0101},{384, 	0x003F0102},{192,	0x003F0103},
+{1560,	0x00400100},{780, 	0x00400101},{390, 	0x00400102},{195,	0x00400103},
+{1584,	0x00410100},{792, 	0x00410101},{396, 	0x00410102},{198,	0x00410103},
+{1608,	0x00420100},{804, 	0x00420101},{402, 	0x00420102},{201,	0x00420103},
+{1632,	0x00430100},{816, 	0x00430101},{408, 	0x00430102},{204,	0x00430103},
+{1656,	0x00440100},{828, 	0x00440101},{414, 	0x00440102},{207,	0x00440103},
+{1680,	0x00450100},{840, 	0x00450101},{420, 	0x00450102},{210,	0x00450103},
+{1704,	0x00460100},{852, 	0x00460101},{426, 	0x00460102},{213,	0x00460103},
+{1728,	0x00470100},{864, 	0x00470101},{432, 	0x00470102},{216,	0x00470103},
+{1752,	0x00480100},{876, 	0x00480101},{438, 	0x00480102},{219,	0x00480103},
+{1776,	0x00490100},{888, 	0x00490101},{444, 	0x00490102},{222,	0x00490103},
+{1800,	0x004A0100},{900, 	0x004A0101},{450, 	0x004A0102},{225,	0x004A0103},
+{1824,	0x004B0100},{912, 	0x004B0101},{456, 	0x004B0102},{228,	0x004B0103},
+{1848,	0x004C0100},{924, 	0x004C0101},{462, 	0x004C0102},{231,	0x004C0103},
+{1872,	0x004D0100},{936, 	0x004D0101},{468, 	0x004D0102},{234,	0x004D0103},
+{1896,	0x004E0100},{948, 	0x004E0101},{474, 	0x004E0102},{237,	0x004E0103},
+{1920,	0x004F0100},{960, 	0x004F0101},{480, 	0x004F0102},{240,	0x004F0103},
+{1944,	0x00500100},{972, 	0x00500101},{486, 	0x00500102},{243,	0x00500103},
+{1968,	0x00510100},{984, 	0x00510101},{492, 	0x00510102},{246,	0x00510103},
+{1992,	0x00520100},{996, 	0x00520101},{498, 	0x00520102},{249,	0x00520103},
+{2016,	0x00530100},{1008, 	0x00530101},{504, 	0x00530102},{252,	0x00530103},
+
+
+{1008,	0x00140000},	{504, 	0x00140001},	{252, 	0x00140002},	{126,	0x00140003},
+{1056,	0x00150000},	{528, 	0x00150001},	{264, 	0x00150002},	{132,	0x00150003},
+{1104,	0x00160000},	{552, 	0x00160001},	{276, 	0x00160002},	{138,	0x00160003},
+{1152,	0x00170000},	{576, 	0x00170001},	{288, 	0x00170002},	{144,	0x00170003},
+{1200,	0x00180000},	{600, 	0x00180001},	{300, 	0x00180002},	{150,	0x00180003},
+{1248,	0x00190000},	{624, 	0x00190001},	{312, 	0x00190002},	{156,	0x00190003},
+{1296,	0x001A0000},	{648, 	0x001A0001},	{324, 	0x001A0002},	{162,	0x001A0003},
+{1344,	0x001B0000},	{672, 	0x001B0001},	{336, 	0x001B0002},	{168,	0x001B0003},
+{1392,	0x001C0000},	{696, 	0x001C0001},	{348, 	0x001C0002},	{174,	0x001C0003},
+{1440,	0x001D0000},	{720, 	0x001D0001},	{360, 	0x001D0002},	{180,	0x001D0003},
+{1488,	0x001E0000},	{744, 	0x001E0001},	{372, 	0x001E0002},	{186,	0x001E0003},
+{1536,	0x001F0000},	{768, 	0x001F0001},	{384, 	0x001F0002},	{192,	0x001F0003},
+{1584,	0x00200000},	{792, 	0x00200001},	{396, 	0x00200002},	{198,	0x00200003},
+{1632,	0x00210000},	{816, 	0x00210001},	{408, 	0x00210002},	{204,	0x00210003},
+{1680,	0x00220000},	{840, 	0x00220001},	{420, 	0x00220002},	{210,	0x00220003},
+{1728,	0x00230000},	{864, 	0x00230001},	{432, 	0x00230002},	{216,	0x00230003},
+{1776,	0x00240000},	{888, 	0x00240001},	{444, 	0x00240002},	{222,	0x00240003},
+{1824,	0x00250000},	{912, 	0x00250001},	{456, 	0x00250002},	{228,	0x00250003},
+{1872,	0x00260000},	{936, 	0x00260001},	{468, 	0x00260002},	{234,	0x00260003},
+{1920,	0x00270000},	{960, 	0x00270001},	{480, 	0x00270002},	{240,	0x00270003},
+{1968,	0x00280000},	{984, 	0x00280001},	{492, 	0x00280002},	{246,	0x00280003},
+{2016,	0x00290000},	{1008, 	0x00290001},	{504, 	0x00290002},	{252,	0x00290003},
+
+{144,	0x00020000},	
+{192,	0x00030000},
+{240,	0x00040000},
+{288,	0x00050000},	{144, 	0x00050001},
+{336,	0x00060000},	{168, 	0x00060001},
+{384,	0x00070000},	{192, 	0x00070001},
+{432,	0x00080000},	{216, 	0x00080001},
+{480,	0x00090000},	{240, 	0x00090001},
+{528,	0x000A0000},	{264, 	0x000A0001},	{132, 	0x000A0002},
+{576,	0x000B0000},	{288, 	0x000B0001},	{144, 	0x000B0002},
+{624,	0x000C0000},	{312, 	0x000C0001},	{156, 	0x000C0002},
+{672,	0x000D0000},	{336, 	0x000D0001},	{168, 	0x000D0002},
+{720,	0x000E0000},	{360, 	0x000E0001},	{180, 	0x000E0002},
+{768,	0x000F0000},	{384, 	0x000F0001},	{192, 	0x000F0002},
+{816,	0x00100000},	{408, 	0x00100001},	{204, 	0x00100002},
+{864,	0x00110000},	{432, 	0x00110001},	{216, 	0x00110002},
+{912,	0x00120000},	{456, 	0x00120001},	{228, 	0x00120002},
+{960,	0x00130000},	{480, 	0x00130001},	{240, 	0x00130002},
+
+{144,	0x00050100},
+{168,	0x00060100},
+{192,	0x00070100},
+{216,	0x00080100},
+{240,	0x00090100},
+{264,	0x000A0100},	{132, 	0x000A0101},	
+{288,	0x000B0100},	{144, 	0x000B0101},	
+{312,	0x000C0100},	{156, 	0x000C0101},	
+{336,	0x000D0100},	{168, 	0x000D0101},	
+{360,	0x000E0100},	{180, 	0x000E0101},	
+{384,	0x000F0100},	{192, 	0x000F0101},	
+{408,	0x00100100},	{204, 	0x00100101},	
+{432,	0x00110100},	{216, 	0x00110101},	
+{456,	0x00120100},	{228, 	0x00120101},	
+{480,	0x00130100},	{240, 	0x00130101},	
+{504,	0x00140100},	{252, 	0x00140101},	{126, 	0x00140102},
+{528,	0x00150100},	{264, 	0x00150101},	{132, 	0x00150102},
+{552,	0x00160100},	{276, 	0x00160101},	{138, 	0x00160102},
+{576,	0x00170100},	{288, 	0x00170101},	{144, 	0x00170102},
+{600,	0x00180100},	{300, 	0x00180101},	{150, 	0x00180102},
+{624,	0x00190100},	{312, 	0x00190101},	{156, 	0x00190102},
+{648,	0x001A0100},	{324, 	0x001A0101},	{162, 	0x001A0102},
+{672,	0x001B0100},	{336, 	0x001B0101},	{168, 	0x001B0102},
+{696,	0x001C0100},	{348, 	0x001C0101},	{174, 	0x001C0102},
+{720,	0x001D0100},	{360, 	0x001D0101},	{180, 	0x001D0102},
+{744,	0x001E0100},	{372, 	0x001E0101},	{186, 	0x001E0102},
+{768,	0x001F0100},	{384, 	0x001F0101},	{192, 	0x001F0102},
+{792,	0x00200100},	{396, 	0x00200101},	{198, 	0x00200102},
+{816,	0x00210100},	{408, 	0x00210101},	{204, 	0x00210102},
+{840,	0x00220100},	{420, 	0x00220101},	{210, 	0x00220102},
+{864,	0x00230100},	{432, 	0x00230101},	{216, 	0x00230102},
+{888,	0x00240100},	{444, 	0x00240101},	{222, 	0x00240102},
+{912,	0x00250100},	{456, 	0x00250101},	{228, 	0x00250102},
+{936,	0x00260100},	{468, 	0x00260101},	{234, 	0x00260102},
+{960,	0x00270100},	{480, 	0x00270101},	{240, 	0x00270102},
+{984,	0x00280100},	{492, 	0x00280101},	{246, 	0x00280102}
+};
+
+#ifndef ARRAYSIZE
+#define ARRAYSIZE(a)           (sizeof(a) / sizeof(a[0]))
+#endif
+#define MAXPLL 3515625*(1<<8)
+unsigned int tblpll = MAXPLL/1000000;
+unsigned int fr_rdn = 0;
+static DEFINE_SPINLOCK(wmt_clk_irqlock);
+unsigned long wmt_clk_irqlock_flags;
+
+void wmt_mc5_autotune(void)//gri
+{
+	unsigned int reg_val, reg_tmp;
+	
+	MC_CLOCK_CTRL0_VAL = 0x0;
+	wmb();
+	MC_CLOCK_CTRL0_VAL = 0x80000000;
+	while(MC_CLOCK_CTRL0_VAL != 0xC0000000);
+	if ((MC_CONF_VAL & 0x3) == 0x2) {
+		//ddr3
+		reg_val = (MC_CLOCK_CTRL1_VAL & 0xffff8080);
+		reg_tmp = (0x7f0000 & reg_val);
+		reg_tmp |= ((0x7f0000 & reg_val) >> 8);
+		reg_tmp |= ((0x7f0000 & reg_val) >> 16);
+		reg_val |= reg_tmp;
+		MC_CLOCK_CTRL1_VAL = reg_val;
+	} else {
+		//lpddr2
+		reg_val = (MC_CLOCK_CTRL1_VAL & 0xffff0000);
+		reg_tmp = (0x7f000000 & reg_val);
+		reg_tmp |= ((0x7f000000 & reg_val) >> 16);
+		reg_tmp |= ((0x7f000000 & reg_val) >> 24);
+		reg_val |= reg_tmp;
+		MC_CLOCK_CTRL1_VAL = reg_val;	
+	}
+}
+
+void wmt_clk_mutex_lock(int lock)
+{
+	if (lock) {
+		spin_lock_irqsave(&wmt_clk_irqlock, wmt_clk_irqlock_flags);
+	}	else {
+		spin_unlock_irqrestore(&wmt_clk_irqlock, wmt_clk_irqlock_flags);
+	}
+}
+
+static void check_PLL_DIV_busy(void)
+{
+	while ((*(volatile unsigned int *)(PMC_BASE+0x18))&PLL_BUSY)
+		;
+}
+
+#ifdef debug_clk
+static void print_refer_count(void)
+{
+	int i;
+	for (i = 0; i < 4; i++) {
+		printk("clk cnt %d ~ %d: %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d %d\n",i*16,(i*16)+15,
+		dev_en_count[i*16],dev_en_count[i*16+1],dev_en_count[i*16+2],dev_en_count[i*16+3],dev_en_count[i*16+4],dev_en_count[i*16+5],dev_en_count[i*16+6],dev_en_count[i*16+7],
+		dev_en_count[i*16+8],dev_en_count[i*16+9],dev_en_count[i*16+10],dev_en_count[i*16+11],dev_en_count[i*16+12],dev_en_count[i*16+13],dev_en_count[i*16+14],dev_en_count[i*16+15]);
+	}
+}
+#endif
+#if 0
+static int disable_dev_clk(enum dev_id dev)
+{
+	int en_count;
+
+	if (dev >= 128) {
+		printk(KERN_INFO"device dev_id = %d > 128\n",dev);
+		return -1;
+	}
+
+	#ifdef debug_clk
+	print_refer_count();
+	#endif
+
+	en_count = dev_en_count[dev];
+	
+	if (en_count <= 1) {
+		dev_en_count[dev] = en_count = 0;
+		*(volatile unsigned int *)(PMC_CLK + 4*(dev/32))
+		&= ~(1 << (dev - 32*(dev/32)));
+		*(volatile unsigned char *)(PMC_BASE + 0x604) = 0;
+		while((REG32_VAL((PMC_BASE + 0x604)) & 0xf5) != 0x00){};
+		*(volatile unsigned char *)(PMC_BASE + 0x62C) = 0;
+		while((REG32_VAL((PMC_BASE + 0x62C)) & 0xf5) != 0x00){};
+		/*printk(KERN_INFO" really disable clock\n");*/
+	} else if (en_count > 1) {
+		dev_en_count[dev] = (--en_count);
+	}
+
+	#ifdef debug_clk
+	print_refer_count();
+	#endif
+
+	return en_count;
+}
+#endif
+/*
+* ON : clock enable : 1, clock disable : 0
+*
+*/
+static int enable_dev_clk(enum dev_id dev, int ON)
+{
+	int en_count, tmp;
+
+	if (dev > 128) {
+		printk(KERN_INFO"device dev_id = %d > 128\n",dev);
+		return -1;
+	}
+
+	#ifdef debug_clk
+	printk(KERN_INFO"device dev_id = %d\n",dev);
+	print_refer_count();
+	#endif
+
+	//mutex_lock(&clk_cnt_lock);
+	wmt_clk_mutex_lock(1);
+	en_count = dev_en_count[dev];
+	
+	if (ON) {
+		tmp = *(volatile unsigned int *)(PMC_CLK + 4*(dev/32));
+		if (en_count <= 0 || !(tmp&(1 << (dev - 32*(dev/32))))) {
+			dev_en_count[dev] = en_count = 1;
+			*(volatile unsigned int *)(PMC_CLK + 4*(dev/32))
+			|= 1 << (dev - 32*(dev/32));
+			/*if (dev == DEV_MSVD) {
+				*(volatile unsigned char *)(PMC_BASE + 0x604) = 1;
+				while((REG32_VAL((PMC_BASE + 0x604)) & 0xf5) != 0xf1){};
+				*(volatile unsigned char *)(PMC_BASE + 0x62C) = 1;
+				while((REG32_VAL((PMC_BASE + 0x62C)) & 0xf5) != 0xf1){};
+			}*/
+			/*printk(KERN_INFO" really enable clock\n");*/
+		} else
+			dev_en_count[dev] = (++en_count);
+	} else {
+		if (en_count <= 1) {
+			dev_en_count[dev] = en_count = 0;
+			*(volatile unsigned int *)(PMC_CLK + 4*(dev/32))
+			&= ~(1 << (dev - 32*(dev/32)));
+			/**(volatile unsigned char *)(PMC_BASE + 0x604) = 0;
+			while((REG32_VAL((PMC_BASE + 0x604)) & 0xf5) != 0x00){};
+			*(volatile unsigned char *)(PMC_BASE + 0x62C) = 0;
+			while((REG32_VAL((PMC_BASE + 0x62C)) & 0xf5) != 0x00){};*/
+			/*printk(KERN_INFO" really disable clock\n");*/
+		} else if (en_count > 1) {
+			dev_en_count[dev] = (--en_count);
+		}
+	}
+	//mutex_unlock(&clk_cnt_lock);
+	wmt_clk_mutex_lock(0);
+
+	#ifdef debug_clk
+	print_refer_count();
+	#endif
+
+	/* mmfreq */
+	if ((dev == MMFREQ_VD) && (mmfreq_num != ~0)) {
+		int cnt;
+		wmt_clk_mutex_lock(1);
+		cnt = dev_en_count[MMFREQ_VD];
+		wmt_clk_mutex_lock(0);
+		if (cnt == 0)
+			wmt_set_mmfreq(mmfreq_num);
+	}
+
+	return en_count;
+}
+/*
+* PLLA return 0, PLLB return 1,
+* PLLC return 2, PLLD return 3,
+* PLLE return 4, PLLF return 5,
+* PLLG return 6,
+* device not found return -1.
+* device has no divisor but has clock enable return -2.
+*/
+static int calc_pll_num(enum dev_id dev, int *div_offset)
+{
+	switch (dev) {
+		case DEV_UART0:
+		case DEV_UART1:
+		case DEV_UART2:
+		case DEV_UART3:		/*case DEV_UART4:		case DEV_UART5:*/
+		case DEV_AHBB:
+		case DEV_CIR:
+		case DEV_SYS:		/* no use */
+		case DEV_RTC:
+		case DEV_UHDC:
+		case DEV_PERM:	/* no use */
+		case DEV_PDMA:	/* no use */
+		case DEV_VID:
+		*div_offset = 0;
+		return -2;
+		case DEV_SAE:
+		*div_offset = 0x348;
+		return 1;
+		case DEV_C24MOUT:
+		*div_offset = 0x3E4;
+		return 1;
+		case DEV_I2C0:
+		*div_offset = 0x3A0;
+		return 1;
+		case DEV_I2C1:
+		*div_offset = 0x3A4;
+		return 1;
+		case DEV_I2C2:
+		*div_offset = 0x3A8;
+		return 1;
+		case DEV_I2C3:
+		*div_offset = 0x3AC;
+		return 1;
+		case DEV_I2C4:
+		*div_offset = 0x39C;
+		return 1;
+		case DEV_PWM:
+		*div_offset = 0x350;
+		return 1;
+		case DEV_SPI0:
+		*div_offset = 0x340;
+		return 1;
+		case DEV_SPI1:
+		*div_offset = 0x344;
+		return 1;
+		case DEV_HDMILVDS:
+		case DEV_SDTV:
+		case DEV_HDMI:
+		*div_offset = 0x36C;
+		return 2;
+		case DEV_DVO:
+		case DEV_LVDS:
+		*div_offset = 0x370;
+		return 6;
+		case DEV_CSI0:
+		*div_offset = 0x380;
+		return 1;
+		case DEV_CSI1:
+		*div_offset = 0x384;
+		return 1;
+		case DEV_MALI:
+		*div_offset = 0x388;
+		return 1;
+		case DEV_DDRMC:
+		*div_offset = 0x310;
+		return 3;
+		case DEV_WMTNA:
+		*div_offset = 0x358;
+		return 3;
+		case DEV_CNMNA:
+		*div_offset = 0x360;
+		return 3;
+		case DEV_WMTVDU:
+		case DEV_JENC:
+		case DEV_HDCE:
+		case DEV_H264:
+		case DEV_JDEC:
+		case DEV_MSVD:
+		case DEV_AMP:
+		case DEV_VPU:
+		case DEV_MBOX:
+		*div_offset = 0x368;
+		return 3;
+		case DEV_CNMVDU:
+		case DEV_VP8DEC:
+		*div_offset = 0x38C;
+		return 3;
+		case DEV_NA12:
+		case DEV_VPP:
+		case DEV_GE:
+		case DEV_DISP:
+		case DEV_GOVRHD:
+		case DEV_SCL444U:
+		case DEV_GOVW:
+		*div_offset = 0x35C;
+		return 1;
+		case DEV_PAXI:
+		case DEV_DMA:
+		*div_offset = 0x354;
+		return 1;
+		case DEV_L2C:
+		*div_offset = 0x30C;
+		return 0;
+		case DEV_L2CAXI:
+		*div_offset = 0x3B0;
+		return 0;
+		case DEV_AT:
+		*div_offset = 0x3B4;
+		return 0;
+		case DEV_PERI:
+		*div_offset = 0x3B8;
+		return 0;
+		case DEV_TRACE:
+		*div_offset = 0x3BC;
+		return 0;
+		case DEV_L2CPAXI:
+		*div_offset = 0x3C0;
+		return 0;
+		case DEV_DBG:
+		*div_offset = 0x3D0;
+		return 0;
+		case DEV_NAND:
+		*div_offset = 0x318;
+		return 1;
+		/*case DEV_NOR:
+		*div_offset = 0x31C;
+		return 1;*/
+		case DEV_SDMMC0:
+		*div_offset = 0x330;
+		return 1;
+		case DEV_SDMMC1:
+		*div_offset = 0x334;
+		return 1;
+		case DEV_SDMMC2:
+		*div_offset = 0x338;
+		return 1;
+		/*case DEV_SDMMC3:
+		*div_offset = 0x33C;
+		return 1;*/
+		case DEV_ADC:
+		*div_offset = 0x394;
+		return 1;
+		case DEV_HDMII2C: /* div 1~63 */
+		*div_offset = 0x390;
+		return 4;
+		case DEV_SF:
+		*div_offset = 0x314;
+		return 1;
+		case DEV_PCM0:
+		*div_offset = 0x324;
+		return 1;
+		case DEV_PCM1:
+		*div_offset = 0x328;
+		return 1;
+		case DEV_I2S:
+		case DEV_ARF:
+		case DEV_ARFP:
+		*div_offset = 0x374;
+		return 4;
+		case DEV_ARM:
+		*div_offset = 0x300;
+			return 0;
+		case DEV_AHB:
+		*div_offset = 0x304;
+			return 1;
+		case DEV_APB:
+		case DEV_SCC:
+		case DEV_GPIO:
+		*div_offset = 0x320;
+			return 1;
+		/*case DEV_ETHPHY:
+		*div_offset = 25;
+			return -2;*/
+		default:
+		return -1;
+	}
+}
+
+
+/* if the clk of the dev is not enable, then enable it or fail */
+int check_clk_enabled(enum dev_id dev, enum clk_cmd cmd) {
+	unsigned int pmc_clk_en;
+	if (dev < 128) {
+		pmc_clk_en = *(volatile unsigned int *)(PMC_CLK + 4*(dev/32));
+		if (!(pmc_clk_en & (1 << (dev - 32*(dev/32))))) {
+			if (dev == DEV_ARM)
+				return 0;
+			/*enable_dev_clk(dev);*/
+			printk(KERN_INFO"dev[%d] clock disabled\n", dev);
+			return -1;
+		}
+	}
+	return 0;
+}
+#if 0
+static int get_freq(enum dev_id dev, int *divisor) {
+
+	unsigned int div = 0, freq;
+	int PLL_NO, j = 0, div_addr_offs;
+	unsigned long long freq_llu, base = 1000000, tmp, mod;
+
+	PLL_NO = calc_pll_num(dev, &div_addr_offs);
+	if (PLL_NO == -1) {
+		printk(KERN_INFO"device not found");
+		return -1;
+	}
+
+	if (PLL_NO == -2)
+		return div_addr_offs*1000000;
+
+	*(volatile unsigned char *)(PMC_BASE + div_addr_offs + 2) = (dev == DEV_SDTV) ? 1 : 0;
+
+	check_PLL_DIV_busy();
+
+	div = *divisor = *(volatile unsigned char *)(PMC_BASE + div_addr_offs);
+	//printk(KERN_INFO"div_addr_offs=0x%x PLL_NO=%d PMC_PLL=0x%x\n", div_addr_offs, PLL_NO, PMC_PLL);
+	tmp = *(volatile unsigned int *)(PMC_PLL + 4*PLL_NO);
+	
+	if ((dev != DEV_SDMMC0) && (dev != DEV_SDMMC1) && (dev != DEV_SDMMC2)) {
+		div = div&0x1F;
+	} else {
+		if (div & (1<<5))
+			j = 1;
+		div &= 0x1F;
+		div = div*(j?64:1)*2;
+	}
+
+	freq_llu = (SRC_FREQ*GET_DIVF(tmp)) * base;
+	//printk(KERN_INFO" freq_llu =%llu\n", freq_llu);
+	tmp = GET_DIVR(tmp) * GET_DIVQ(tmp) * div;
+	mod = do_div(freq_llu, tmp);
+	freq = (unsigned int)freq_llu;
+	//printk("get_freq cmd: freq=%d freq(unsigned long long)=%llu div=%llu mod=%llu\n", freq, freq_llu, tmp, mod);
+
+	return freq;
+}
+#endif
+extern unsigned int wmt_read_oscr(void);
+static int get_freq_t(enum dev_id dev, int *divisor) {
+
+	unsigned int div = 0, freq = 0, div_ahb = 1;
+	int PLL_NO, i, j = 0, div_addr_offs;
+	unsigned long long freq_llu = 0, base = 1000000, tmp, mod;
+	#ifdef DEBUG_CLK
+	unsigned int t1 = 0, t2 = 0;
+	#endif
+
+	if (dev == DEV_APB) {
+		PLL_NO = calc_pll_num(DEV_AHB, &div_addr_offs);
+		if (PLL_NO < 0) {
+			printk(KERN_INFO"device not found");
+			return -1;
+		}
+		div_ahb = *(volatile unsigned char *)(PMC_BASE + div_addr_offs);
+	}
+
+	PLL_NO = calc_pll_num(dev, &div_addr_offs);
+	if (PLL_NO == -1) {
+		printk(KERN_INFO"device not found");
+		return -1;
+	}
+
+	if (PLL_NO == -2)
+		return div_addr_offs*1000000;
+
+	*(volatile unsigned char *)(PMC_BASE + div_addr_offs + 2) = (dev == DEV_SDTV) ? 1 : 0;
+
+	check_PLL_DIV_busy();
+
+	div = *divisor = *(volatile unsigned char *)(PMC_BASE + div_addr_offs);
+
+	if ((dev != DEV_SDMMC0) && (dev != DEV_SDMMC1) && (dev != DEV_SDMMC2)) {
+		div = div&0x1F;
+	} else {
+		if (div & (1<<5))
+			j = 1;
+		div &= 0x1F;
+		div = div*(j?64:1)*2;
+	}
+
+	#ifdef DEBUG_CLK
+	printk(KERN_INFO"div_addr_offs=0x%x PLL_NO=%d PMC_PLL=0x%x\n", div_addr_offs, PLL_NO, PMC_PLL);
+	t1 = wmt_read_oscr();
+	#endif
+	//mutex_lock(&clk_cnt_lock);
+	tmp = *(volatile unsigned int *)(PMC_PLL + 4*PLL_NO);
+	//mutex_unlock(&clk_cnt_lock);
+	for (i = 0; i < ARRAYSIZE(pllmapAll); i++) {
+		if (pllmapAll[i].pll == tmp) {
+			freq = pllmapAll[i].freq;
+			#ifdef DEBUG_CLK
+			printk("********dev%d---PLL_NO=%X---get freq=%d set0x%x\n",	dev, PLL_NO+10, freq, pllmapAll[i].pll);
+			#endif
+			break;
+		}
+	}
+	if (i >= ARRAYSIZE(pllmapAll) || freq == 0) {
+		printk(KERN_INFO"gfreq : dev%d********************pll not match table**************\n", dev);
+		freq_llu = (SRC_FREQ*GET_DIVF(tmp)) * base;
+		//printk(KERN_INFO" freq_llu =%llu\n", freq_llu);
+		tmp = GET_DIVR(tmp) * GET_DIVQ(tmp) * div * div_ahb;
+		mod = do_div(freq_llu, tmp);
+		freq = (unsigned int)freq_llu;
+	} else {
+		freq = (freq * 15625)<<6;
+		freq = freq/(div*div_ahb);
+	}
+
+	#ifdef DEBUG_CLK
+	t2 = wmt_read_oscr() - t1;
+	printk("************delay_time=%d\n", t2);
+	printk("get_freq cmd: freq=%d freq(unsigned long long)=%llu div=%llu mod=%llu\n",
+	freq, freq_llu, tmp, mod);
+	#endif
+
+	#ifdef DEBUG_PLLA
+	if (dev == DEV_ARM && fr_rdn > (tblpll*1000000))
+		return fr_rdn;
+	#endif
+
+	return freq;
+}
+#if 0
+static int set_divisor(enum dev_id dev, int unit, int freq, int *divisor) {
+
+	unsigned int div = 0, tmp;
+	int PLL_NO, i, j = 0, div_addr_offs, SD_MMC = 1, ret;
+	unsigned long long base = 1000000, PLL, PLL_tmp, mod, div_llu, freq_target = freq, mini,tmp1;
+	if ((dev != DEV_SDMMC0) && (dev != DEV_SDMMC1) && (dev != DEV_SDMMC2))
+		SD_MMC = 0;
+
+	PLL_NO = calc_pll_num(dev, &div_addr_offs);
+	if (PLL_NO == -1) {
+		printk(KERN_INFO"device not found");
+		return -1;
+	}
+
+	if (PLL_NO == -2) {
+		printk(KERN_INFO"device has no divisor");
+		return -1;
+	}
+	
+	*(volatile unsigned char *)(PMC_BASE + div_addr_offs + 2) = (dev == DEV_SDTV) ? 1 : 0;
+
+	check_PLL_DIV_busy();
+
+	tmp = *(volatile unsigned int *)(PMC_PLL + 4*PLL_NO);
+	PLL = SRC_FREQ*GET_DIVF(tmp)*base;
+	//printk(KERN_INFO" PLL =%llu, dev_id=%d, PLL_NO=%d, PMC_PLL=0x%x, tmp = 0x%x\n", PLL, dev, PLL_NO,PMC_PLL,(unsigned int)&tmp);
+	div_llu = GET_DIVR(tmp)*GET_DIVQ(tmp);
+
+	ret = check_clk_enabled(dev, SET_DIV);
+	if (ret)
+		return -1;
+
+	if (unit == 1)
+		freq_target *= 1000;
+	else if (unit == 2)
+		freq_target *= 1000000;
+
+	if (SD_MMC == 0) {
+		for (i = 1; i < 33; i++) {
+			if ((i > 1 && (i%2)) && (PLL_NO == 2))
+				continue;
+				PLL_tmp = PLL;
+				mod = do_div(PLL_tmp, div_llu*i);
+			if (PLL_tmp <= freq_target) {
+				*divisor = div = i;
+				break;
+			}
+		}
+	} else {
+		mini = freq_target;
+		tmp1 = PLL;
+		do_div(tmp1, div_llu);
+		do_div(tmp1, 64);
+		if ((tmp1) >= freq_target)
+				j = 1;
+		for (i = 1; i < 33; i++) {
+			PLL_tmp = PLL;
+			mod = do_div(PLL_tmp, div_llu*(i*(j?64:1)*2));
+		if (PLL_tmp <= freq_target) {
+			*divisor = div = i;
+				break;
+			}
+		}
+	}
+	if (div != 0 && div < 33) {
+		check_PLL_DIV_busy();
+		if (dev == DEV_WMTNA)
+			*(volatile unsigned int *)(PMC_BASE + div_addr_offs) =	(0x200 | ((div == 32) ? 0 : div));
+		else
+			*(volatile unsigned int *)(PMC_BASE + div_addr_offs)
+			=	((dev == DEV_SDTV)?0x10000:0) + (j?32:0) + ((div == 32) ? 0 : div);
+		check_PLL_DIV_busy();
+		freq = (unsigned int)PLL_tmp;
+		/*printk("set_divisor: freq=%d freq(unsigned long long)=%llu div=%llu mod=%llu divisor%d pmc_pll=0x%x\n",
+		freq, PLL_tmp, div_llu, mod, *divisor, tmp);*/
+		return freq;
+	}
+	printk(KERN_INFO"no suitable divisor");
+	return -1;
+}
+#endif
+static int set_divisor_t(enum dev_id dev, int unit, int freq, int *divisor) {
+
+	unsigned int div = 0, tmp;
+	int /*ret,*/ PLL_NO, i, j = 1, div_addr_offs, SD_MMC = 1;
+	unsigned int PLL = 0, freq_target = freq;
+	if ((dev != DEV_SDMMC0) && (dev != DEV_SDMMC1) && (dev != DEV_SDMMC2))
+		SD_MMC = 0;
+
+	PLL_NO = calc_pll_num(dev, &div_addr_offs);
+	if (PLL_NO == -1) {
+		printk(KERN_INFO"device not found");
+		return -1;
+	} else if (PLL_NO == -2) {
+		printk(KERN_INFO"device has no divisor");
+		return -1;
+	}
+
+	*(volatile unsigned char *)(PMC_BASE + div_addr_offs + 2) = (dev == DEV_SDTV) ? 1 : 0;
+
+	check_PLL_DIV_busy();
+	tmp = *(volatile unsigned int *)(PMC_PLL + 4*PLL_NO);
+
+	/*ret = check_clk_enabled(dev, SET_DIV);
+	if (ret)
+		return -1;*/
+
+	if (unit == 1)
+		freq_target *= 1000;
+	else if (unit == 2)
+		freq_target *= 1000000;
+
+	for (i = 0; i < ARRAYSIZE(pllmapAll); i++) {
+		if (pllmapAll[i].pll == tmp) {
+			PLL = pllmapAll[i].freq;
+			break;
+		}
+	}
+	if (i >= ARRAYSIZE(pllmapAll) || PLL == 0) {
+		printk(KERN_INFO"set div : dev%d********************pll not match table**************\n", dev);
+		PLL = (SRC_FREQ/GET_DIVQ(tmp)) * (GET_DIVF(tmp)/GET_DIVR(tmp));
+	}
+	PLL = (PLL * 15625)<<6;
+	
+	if (SD_MMC == 0) {
+		for (i = 1; i < 64; i++) {
+			if ((i > 1 && (i%2)) && (PLL_NO == 2))
+				continue;
+				freq = PLL/i;
+			if (freq <= freq_target) {
+				*divisor = div = i;
+				break;
+			}
+		}
+	} else {
+		if ((PLL>>6) >= freq_target)
+			j = 1<<6;
+		for (i = 1; i < 64; i++) {
+			freq = PLL/(i*j);
+			if (freq <= freq_target) {
+				*divisor = div = i;
+				break;
+			}
+		}
+	}
+	if (div > 32)
+		div = 32;
+
+	if (div != 0 && div < 33) {
+		//mutex_lock(&clk_cnt_lock);
+		check_PLL_DIV_busy();
+		if (dev == DEV_WMTNA)
+			*(volatile unsigned int *)(PMC_BASE + div_addr_offs) =	(0x200 | ((div == 32) ? 0 : div));
+		else
+			*(volatile unsigned int *)(PMC_BASE + div_addr_offs)
+			=	((dev == DEV_SDTV)?0x10000:0) + (j==64?32:0) + ((div == 32) ? 0 : div);
+		#ifdef DEBUG_CLK
+		printk("SETDIV********dev%d PLL_%X PLL=0x%x div%d cal_freq=%d target_freq%d\n",
+		dev, PLL_NO+10, PLL, div, freq, freq_target);
+		#endif
+		check_PLL_DIV_busy();
+		//mutex_unlock(&clk_cnt_lock);
+		return freq;
+	}
+	printk(KERN_INFO"no suitable divisor\n");
+	return -1;
+}
+#if 0
+static int set_pll_speed(enum dev_id dev, int unit, int freq, int *divisor, int wait) {
+
+	unsigned int PLL, DIVF=1, DIVR=1, DIVQ=1;
+	unsigned int last_freq, div=1;
+	unsigned long long minor, min_minor = 0xFF000000;
+	int PLL_NO, div_addr_offs, DF, DR, VD, DQ;
+	unsigned long long base = 1000000, base_f = 1, PLL_llu, div_llu, freq_llu, mod;
+	PLL_NO = calc_pll_num(dev, &div_addr_offs);
+	if (PLL_NO == -1) {
+		printk(KERN_INFO"device not belong to PLL A B C D E");
+		return -1;
+	}
+
+	if (PLL_NO == -2) {
+		printk(KERN_INFO"device has no divisor");
+		return -1;
+	}
+
+	*(volatile unsigned char *)(PMC_BASE + div_addr_offs + 2) = (dev == DEV_SDTV) ? 1 : 0;
+
+	check_PLL_DIV_busy();
+	VD = *divisor = *(volatile unsigned char *)(PMC_BASE + div_addr_offs);
+
+	if (unit == 1)
+		base_f = 1000;
+	else if (unit == 2)
+		base_f = 1000000;
+	else if (unit != 0) {
+		printk(KERN_INFO"unit is out of range");
+		return -1;
+	}
+	freq_llu = freq * base_f;
+	for (DR = MAX_DR; DR >= 0; DR--) {
+		for (DQ = MAX_DQ; DQ >= 0; DQ--) {
+			for (DF = 0; DF <= MAX_DF; DF++) {
+				if (SRC_FREQ/(DR+1) < 10)
+					break;
+				if ((1000/SRC_FREQ) > ((2*(DF+1))/(DR+1)))
+					continue;
+				if ((2000/SRC_FREQ) < ((2*(DF+1))/(DR+1)))
+					break;
+				PLL_llu = (unsigned long long)(SRC_FREQ * 2 * (DF+1) * base);
+				div_llu = (unsigned long long)(VD * (DR+1)*(1<<DQ));
+				mod = do_div(PLL_llu, div_llu);
+				if (PLL_llu == freq_llu) {
+					DIVF = DF;
+					DIVR = DR;
+					DIVQ = DQ;
+					div = VD;
+					/*printk(KERN_INFO"find the equal value");*/
+					goto find;
+				} else if (PLL_llu < freq_llu) {
+					minor = freq_llu - PLL_llu;
+					//printk(KERN_INFO"minor=0x%x, min_minor=0x%x", minor, min_minor);
+					if (minor < min_minor) {
+						DIVF = DF;
+						DIVR = DR;
+						DIVQ = DQ;
+						div = VD;
+						min_minor = minor;
+					}
+				} else if (PLL_llu > freq_llu) {
+					if (PLL_NO == 2) {
+						minor = PLL_llu - freq_llu;
+						if (minor < min_minor) {
+							DIVF = DF;
+							DIVR = DR;
+							DIVQ = DQ;
+							div = VD;
+							min_minor = minor;
+						}
+					}
+					break;
+				}
+			}//DF
+		}//DQ
+	}//DR
+/*minimun:*/
+	//printk(KERN_INFO"minimum minor=0x%x, unit=%d \n", (unsigned int)min_minor, unit);
+find:
+
+	PLL_llu = (unsigned long long)(SRC_FREQ * 2 * (DIVF+1) * base);
+	div_llu = (unsigned long long)((DIVR+1)*(1<<DIVQ)*(*divisor));
+	mod = do_div(PLL_llu, div_llu);
+	last_freq = (unsigned int)PLL_llu;
+	/*printk("set_pll cmd: freq=%d freq(unsigned long long)=%llu div=%llu mod=%llu\n", last_freq, PLL_llu, div_llu, mod);
+	printk(KERN_INFO"DIVF%d, DIVR%d, DIVQ%d, divisor%d freq=%dHz \n",
+	DIVF, DIVR, DIVQ, *divisor, last_freq);*/
+	PLL = (DIVF<<16) + (DIVR<<8) + DIVQ;
+
+	/* if the clk of the device is not enable, then enable it */
+	/*if (dev < 128) {
+		pmc_clk_en = *(volatile unsigned int *)(PMC_CLK + 4*(dev/32));
+		if (!(pmc_clk_en & (1 << (dev - 32*(dev/32)))))
+			enable_dev_clk(dev);
+	}*/
+
+	check_PLL_DIV_busy();
+	/*printk(KERN_INFO"PLL0x%x, pll addr =0x%x\n", PLL, PMC_PLL + 4*PLL_NO);*/
+	*(volatile unsigned int *)(PMC_PLL + 4*PLL_NO) = PLL;
+	if (wait)
+		check_PLL_DIV_busy();
+	return last_freq;
+
+	printk(KERN_INFO"no suitable pll");
+	return -1;
+}
+#endif
+static int set_pll_speed_t(enum dev_id dev, int unit, int freq, int *divisor, int wait) {
+
+	unsigned int PLL, VD;
+	unsigned int minor, min_minor = 0x7F000000, set_freq = 0;
+	int PLL_NO, div_addr_offs, i;
+	unsigned int tmp_freq, tmp_PLL = 0;
+	PLL_NO = calc_pll_num(dev, &div_addr_offs);
+	if (PLL_NO == -1) {
+		printk(KERN_INFO"device not belong to PLL A B C D E");
+		return -1;
+	}
+
+	if (PLL_NO == -2) {
+		printk(KERN_INFO"device has no divisor");
+		return -1;
+	}
+
+	*(volatile unsigned char *)(PMC_BASE + div_addr_offs + 2) = (dev == DEV_SDTV) ? 1 : 0;
+
+	check_PLL_DIV_busy();
+	VD = *divisor = *(volatile unsigned char *)(PMC_BASE + div_addr_offs);
+
+	if (unit == 1)
+		freq *= 1000;
+	else if (unit == 2)
+		freq *= 1000000;
+	else if (unit != 0) {
+		printk(KERN_INFO"unit is out of range");
+		return -1;
+	}
+	
+	for (i = 0; i < ARRAYSIZE(pllmap); i++) {
+		if (pllmap[i].freq > 1150 && PLL_NO == 4)
+			continue;
+		if (pllmap[i].freq > 1250 && ((PLL_NO == 2) || (PLL_NO == 6)))
+			continue;
+		tmp_freq = ((pllmap[i].freq*15625)<<6) / VD;
+		if (tmp_freq == freq) {
+			tmp_PLL = pllmap[i].pll;
+			set_freq = tmp_freq;
+			min_minor = 0;
+			goto find;
+		} else if (tmp_freq < freq) {
+			minor = freq - tmp_freq;
+			if (minor < min_minor) {
+				tmp_PLL = pllmap[i].pll;
+				set_freq = tmp_freq;
+				min_minor = minor;
+			}
+		} else if (tmp_freq > freq) {
+			if (PLL_NO == 2) {
+				minor = tmp_freq - freq;
+				if (minor < min_minor) {
+					tmp_PLL = pllmap[i].pll;
+					set_freq = tmp_freq;
+					min_minor = minor;
+				}
+			}
+			break;
+		}
+	}
+find:
+	#ifdef DEBUG_CLK
+	if (min_minor)
+		printk(KERN_INFO"minimum minor=0x%x, unit=%d \n", (unsigned int)min_minor, unit);
+	printk("SETPLL********dev%d PLL_%X PLL=0x%x div%d cal_freq=%d target_freq%d\n",
+	dev, PLL_NO+10, tmp_PLL, div, set_freq, freq);
+	#endif
+	PLL = tmp_PLL;
+
+	//mutex_lock(&clk_cnt_lock);
+	check_PLL_DIV_busy();
+	*(volatile unsigned int *)(PMC_PLL + 4*PLL_NO) = PLL;
+	if (wait)
+		check_PLL_DIV_busy();
+	//mutex_unlock(&clk_cnt_lock);
+	
+	return set_freq;
+}
+#if 0
+static int set_pll_divisor(enum dev_id dev, int unit, int freq, int *divisor) {
+
+	unsigned int PLL, DIVF=1, DIVR=1, DIVQ=1, pmc_clk_en, old_divisor;
+	unsigned int last_freq, div=1;
+	unsigned long long minor, min_minor = 0xFF000000;
+	int PLL_NO, div_addr_offs, DF, DR, VD, DQ;
+	unsigned long long base = 1000000, base_f = 1, PLL_llu, div_llu, freq_llu, mod;
+	PLL_NO = calc_pll_num(dev, &div_addr_offs);
+	if (PLL_NO == -1) {
+		printk(KERN_INFO"device not belong to PLL A B C D E");
+		return -1;
+	}
+
+	if (PLL_NO == -2) {
+		printk(KERN_INFO"device has no divisor");
+		return -1;
+	}
+
+	*(volatile unsigned char *)(PMC_BASE + div_addr_offs + 2) = (dev == DEV_SDTV) ? 1 : 0;
+
+	check_PLL_DIV_busy();
+
+	old_divisor = *(volatile unsigned char *)(PMC_BASE + div_addr_offs);
+
+	if (unit == 1)
+		base_f = 1000;
+	else if (unit == 2)
+		base_f = 1000000;
+	else if (unit != 0) {
+		printk(KERN_INFO"unit is out of range");
+		return -1;
+	}
+//printk(KERN_INFO"target freq=%d unit=%d PLL_NO=%d\n", freq, unit, PLL_NO);
+	freq_llu = freq * base_f;
+	//tmp = div * freq;
+	for (DR = MAX_DR; DR >= 0; DR--) {
+		for (DQ = MAX_DQ; DQ >= 0; DQ--) {
+			for (VD = 32; VD >= 1; VD--) {
+				if ((VD > 1 && (VD%2)) && ((PLL_NO == 2) || (PLL_NO == 6)))
+					continue;
+				for (DF = 0; DF <= MAX_DF; DF++) {
+					if (SRC_FREQ/(DR+1) < 10)
+						break;
+					if ((1000/SRC_FREQ) > ((2*(DF+1))/(DR+1)))
+						continue;
+					if ((2000/SRC_FREQ) < ((2*(DF+1))/(DR+1)))
+						break;
+					PLL_llu = (unsigned long long)(SRC_FREQ * 2 * (DF+1) * base);
+					div_llu = (unsigned long long)(VD * (DR+1)*(1<<DQ));
+					mod = do_div(PLL_llu, div_llu);
+					if (PLL_llu == freq_llu) {
+						DIVF = DF;
+						DIVR = DR;
+						DIVQ = DQ;
+						div = VD;
+						/*printk(KERN_INFO"find the equal value");*/
+						goto find;
+					} else if ((PLL_llu < freq_llu) /*&& (dev != DEV_I2S)*/) {
+						minor = freq_llu - PLL_llu;
+						//printk(KERN_INFO"minor=0x%x, min_minor=0x%x", minor, min_minor);
+						if (minor < min_minor) {
+							DIVF = DF;
+							DIVR = DR;
+							DIVQ = DQ;
+							div = VD;
+							min_minor = minor;
+							/*printk(KERN_INFO"< target :DIVF%d, DIVR%d, DIVQ%d, divisor%d min_minor=%lluHz \n",
+							DIVF, DIVR, DIVQ, div, min_minor);*/
+						}
+					} else if (PLL_llu > freq_llu) {
+						if ((PLL_NO == 2) || (PLL_NO == 6)) {
+							minor = PLL_llu - freq_llu;
+							if (minor < min_minor) {
+								DIVF = DF;
+								DIVR = DR;
+								DIVQ = DQ;
+								div = VD;
+								min_minor = minor;
+								/*printk(KERN_INFO" > target DIVF%d, DIVR%d, DIVQ%d, divisor%d min_minor=%lluHz \n",
+								DIVF, DIVR, DIVQ, div, min_minor);*/
+							}
+						}
+						break;
+					}
+				}//DF
+			}//VD
+		}//DQ
+	}//DR
+/*minimun:*/
+	//printk(KERN_INFO"minimum minor=%llu, unit=%d \n", min_minor, unit);
+find:
+
+	*divisor = div;
+
+	PLL_llu = (unsigned long long)(SRC_FREQ * 2 * (DIVF+1) * base);
+	div_llu = (unsigned long long)((DIVR+1)*(1<<DIVQ)*(*divisor));
+	mod = do_div(PLL_llu, div_llu);
+	last_freq = (unsigned int)PLL_llu;
+	/*printk("set_pll_divisor cmd: freq=%d freq(unsigned long long)=%llu div=%llu mod=%llu\n", last_freq, PLL_llu, div_llu, mod);
+	printk(KERN_INFO"DIVF%d, DIVR%d, DIVQ%d, divisor%d freq=%dHz \n",
+	DIVF, DIVR, DIVQ, *divisor, last_freq);*/
+	PLL = (DIVF<<16) + (DIVR<<8) + DIVQ;
+
+	/* if the clk of the device is not enable, then enable it */
+	if (dev < 128) {
+		pmc_clk_en = *(volatile unsigned int *)(PMC_CLK + 4*(dev/32));
+		if (!(pmc_clk_en & (1 << (dev - 32*(dev/32)))) && dev != DEV_ARM) {
+			/*enable_dev_clk(dev);*/
+			printk(KERN_INFO"device clock is disabled");
+			return -1;
+		}
+	}
+
+	check_PLL_DIV_busy();
+	if (old_divisor < *divisor) {
+		if (dev == DEV_WMTNA)
+			*(volatile unsigned int *)(PMC_BASE + div_addr_offs) =	(0x200 | ((div == 32) ? 0 : div));
+		else
+			*(volatile unsigned int *)(PMC_BASE + div_addr_offs)
+			= ((dev == DEV_SDTV)?0x10000:0) +/*(j?32:0) + */((div == 32) ? 0 : div)/* + (div&1) ? (1<<8): 0*/;
+		check_PLL_DIV_busy();
+	}
+	//printk(KERN_INFO"PLL0x%x, pll addr =0x%x\n", PLL, PMC_PLL + 4*PLL_NO);
+	*(volatile unsigned int *)(PMC_PLL + 4*PLL_NO) = PLL;
+	check_PLL_DIV_busy();
+	/*printk(KERN_INFO"DIVF%d, DIVR%d, DIVQ%d, div%d div_addr_offs=0x%x\n",
+	DIVF, DIVR, DIVQ, div, div_addr_offs);*/
+
+	if (old_divisor > *divisor) {
+		if (dev == DEV_WMTNA)
+			*(volatile unsigned int *)(PMC_BASE + div_addr_offs) =	(0x200 | ((div == 32) ? 0 : div));
+		else
+			*(volatile unsigned int *)(PMC_BASE + div_addr_offs)
+			= ((dev == DEV_SDTV)?0x10000:0) +/*(j?32:0) + */((div == 32) ? 0 : div)/* + (div&1) ? (1<<8): 0*/;
+		check_PLL_DIV_busy();
+	}
+
+
+	/*ddv = *(volatile unsigned int *)(PMC_BASE + div_addr_offs);
+	check_PLL_DIV_busy();
+	pllx = *(volatile unsigned int *)(PMC_PLL + 4*PLL_NO);
+	printk(KERN_INFO"read divisor=%d, pll=0x%x from register\n", 0x1F&ddv, pllx);*/
+	return last_freq;
+
+	/*printk(KERN_INFO"no suitable divisor");
+	return -1;*/
+}
+#endif
+
+static int set_pll_divisor_t(enum dev_id dev, int unit, int freq, int *divisor) {
+
+	unsigned int PLL, old_divisor, div=1, set_freq = 0;
+	unsigned int minor, min_minor = 0x7F000000, tmp_freq = 0, tmp_PLL = 0, tfreq = (unsigned int)freq;
+	int i, PLL_NO, div_addr_offs, VD/*, ret*/;
+
+	PLL_NO = calc_pll_num(dev, &div_addr_offs);
+	if (PLL_NO == -1) {
+		printk(KERN_INFO"device not belong to PLL A B C D E");
+		return -1;
+	} else if (PLL_NO == -2) {
+		printk(KERN_INFO"device has no divisor");
+		return -1;
+	}
+
+	*(volatile unsigned char *)(PMC_BASE + div_addr_offs + 2) = (dev == DEV_SDTV) ? 1 : 0;
+
+	check_PLL_DIV_busy();
+	old_divisor = *(volatile unsigned char *)(PMC_BASE + div_addr_offs);
+
+	if (unit == 1)
+		tfreq *= 1000;
+	else if (unit == 2)
+		tfreq *= 1000000;
+	else if (unit != 0) {
+		printk(KERN_INFO"unit is out of range");
+		return -1;
+	}
+	
+	for (i = 0; i < ARRAYSIZE(pllmap); i++) {
+		if (pllmap[i].freq > 1150 && PLL_NO == 4)
+			continue;
+		if (pllmap[i].freq > 1250 && ((PLL_NO == 2) || (PLL_NO == 6)))
+			continue;
+		for (VD = 32; VD >= 1; VD--) {
+			if ((VD > 1 && (VD == 3)) && ((PLL_NO == 2) || (PLL_NO == 6)))
+				continue;
+			tmp_freq = ((pllmap[i].freq*15625)<<6) / VD;
+			if (tmp_freq == tfreq) {
+				tmp_PLL = pllmap[i].pll;
+				set_freq = tmp_freq;
+				div = VD;
+				min_minor = 0;
+				goto find;
+			} else if (tmp_freq < tfreq) {
+				minor = tfreq - tmp_freq;
+				if (minor < min_minor) {
+					tmp_PLL = pllmap[i].pll;
+					set_freq = tmp_freq;
+					div = VD;
+					min_minor = minor;
+				}
+			} else if (tmp_freq > tfreq) {
+				if ((PLL_NO == 2) || (PLL_NO == 6)) {
+					minor = tmp_freq - tfreq;
+					if (minor < min_minor) {
+						tmp_PLL = pllmap[i].pll;
+						set_freq = tmp_freq;
+						div = VD;
+						min_minor = minor;
+					}
+				}
+				break;
+			}
+		}
+	}
+find:
+	#ifdef DEBUG_CLK
+	if (min_minor)
+		printk(KERN_INFO"minimum minor=0x%x, unit=%d \n", (unsigned int)min_minor, unit);
+	printk("SETPLLDIV********dev%d PLL_%X PLL=0x%x div%d cal_freq=%d target_freq%d\n",
+	dev, PLL_NO+10, tmp_PLL, div, set_freq, tfreq);
+	#endif
+
+	*divisor = div;
+	PLL = tmp_PLL;
+
+	/*ret = check_clk_enabled(dev, SET_PLLDIV);
+	if (ret)
+		return -1;*/
+
+	//mutex_lock(&clk_cnt_lock);
+	wmt_clk_mutex_lock(1);
+	check_PLL_DIV_busy();
+	if (old_divisor < *divisor) {
+		if (dev == DEV_WMTNA)
+			*(volatile unsigned int *)(PMC_BASE + div_addr_offs) =	(0x200 | ((div == 32) ? 0 : div));
+		else
+			*(volatile unsigned int *)(PMC_BASE + div_addr_offs)
+			= ((dev == DEV_SDTV)?0x10000:0) +/*(j?32:0) + */((div == 32) ? 0 : div)/* + (div&1) ? (1<<8): 0*/;
+		check_PLL_DIV_busy();
+	}
+	*(volatile unsigned int *)(PMC_PLL + 4*PLL_NO) = PLL;
+	check_PLL_DIV_busy();
+
+	if (old_divisor > *divisor) {
+		if (dev == DEV_WMTNA)
+			*(volatile unsigned int *)(PMC_BASE + div_addr_offs) =	(0x200 | ((div == 32) ? 0 : div));
+		else
+			*(volatile unsigned int *)(PMC_BASE + div_addr_offs)
+			= ((dev == DEV_SDTV)?0x10000:0) +/*(j?32:0) + */((div == 32) ? 0 : div)/* + (div&1) ? (1<<8): 0*/;
+		check_PLL_DIV_busy();
+	}
+	wmt_clk_mutex_lock(0);
+	//mutex_unlock(&clk_cnt_lock);
+
+	return set_freq;
+}
+
+static int get_freq_timer(enum dev_id dev, int *divisor) {
+
+	unsigned int div = 0, freq = 0, div_ahb = 1;
+	int PLL_NO, i, j = 0, div_addr_offs;
+	unsigned long long freq_llu = 0, base = 1000000, tmp, mod;
+	#ifdef DEBUG_CLK
+	unsigned int t1 = 0, t2 = 0;
+	#endif
+
+	if (dev == DEV_APB) {
+		PLL_NO = calc_pll_num(DEV_AHB, &div_addr_offs);
+		if (PLL_NO < 0) {
+			printk(KERN_INFO"device not found");
+			return -1;
+		}
+		div_ahb = *(volatile unsigned char *)(PMC_BASE + div_addr_offs);
+	}
+
+	PLL_NO = calc_pll_num(dev, &div_addr_offs);
+	if (PLL_NO == -1) {
+		printk(KERN_INFO"device not found");
+		return -1;
+	}
+
+	if (PLL_NO == -2)
+		return div_addr_offs*1000000;
+
+	*(volatile unsigned char *)(PMC_BASE + div_addr_offs + 2) = (dev == DEV_SDTV) ? 1 : 0;
+
+	check_PLL_DIV_busy();
+
+	div = *divisor = *(volatile unsigned char *)(PMC_BASE + div_addr_offs);
+
+	if ((dev != DEV_SDMMC0) && (dev != DEV_SDMMC1) && (dev != DEV_SDMMC2)) {
+		div = div&0x1F;
+	} else {
+		if (div & (1<<5))
+			j = 1;
+		div &= 0x1F;
+		div = div*(j?64:1)*2;
+	}
+
+	#ifdef DEBUG_CLK
+	printk(KERN_INFO"div_addr_offs=0x%x PLL_NO=%d PMC_PLL=0x%x\n", div_addr_offs, PLL_NO, PMC_PLL);
+	t1 = wmt_read_oscr();
+	#endif
+	//mutex_lock(&clk_cnt_lock);
+	tmp = *(volatile unsigned int *)(PMC_PLL + 4*PLL_NO);
+	//mutex_unlock(&clk_cnt_lock);
+	for (i = 0; i < ARRAYSIZE(pllmapAll); i++) {
+		if (pllmapAll[i].pll == tmp) {
+			freq = pllmapAll[i].freq;
+			#ifdef DEBUG_CLK
+			printk("********dev%d---PLL_NO=%X---get freq=%d set0x%x\n",	dev, PLL_NO+10, freq, pllmapAll[i].pll);
+			#endif
+			break;
+		}
+	}
+	if (i >= ARRAYSIZE(pllmapAll) || freq == 0) {
+		printk(KERN_INFO"gfreq : dev%d********************pll not match table**************\n", dev);
+		freq_llu = (SRC_FREQ*GET_DIVF(tmp)) * base;
+		//printk(KERN_INFO" freq_llu =%llu\n", freq_llu);
+		tmp = GET_DIVR(tmp) * GET_DIVQ(tmp) * div * div_ahb;
+		mod = do_div(freq_llu, tmp);
+		freq = (unsigned int)freq_llu;
+	} else {
+		freq = (freq * 15625)<<6;
+		freq = freq/(div*div_ahb);
+	}
+
+	#ifdef DEBUG_CLK
+	t2 = wmt_read_oscr() - t1;
+	printk("************delay_time=%d\n", t2);
+	printk("get_freq cmd: freq=%d freq(unsigned long long)=%llu div=%llu mod=%llu\n",
+	freq, freq_llu, tmp, mod);
+	#endif
+
+	return freq;
+}
+
+/*
+* cmd : CLK_DISABLE  : disable clock,
+*       CLK_ENABLE   : enable clock,
+*       GET_FREQ     : get device frequency, it doesn't enable or disable clock,
+*       SET_DIV      : set clock by setting divisor only(clock must be enabled by CLK_ENABLE command),
+*       SET_PLL      : set clock by setting PLL only, no matter clock is enabled or not,
+*                      this cmd can be used before CLK_ENABLE cmd to avoid a extreme speed
+*                      to be enabled when clock enable.
+*       SET_PLLDIV   : set clock by setting PLL and divisor(clock must be enabled by CLK_ENABLE command).
+* dev : Target device ID to be set the clock.
+* unit : the unit of parameter "freq", 0 = Hz, 1 = KHz, 2 = MHz.
+* freq : frequency of the target to be set when cmd is "SET_XXX".
+*
+* return : return value is different depend on cmd type,
+*				CLK_DISABLE  : return internal count which means how many drivers still enable this clock,
+*				               retrun -1 if this device has no clock enable register.
+*				               
+*				CLK_ENABLE   : return internal count which means how many drivers enable this clock,
+*				               retrun -1 if this device has no clock enable register.
+*
+*				GET_FREQ     : return device frequency in Hz when clock is enabled,
+*				               return -1 when clock is disabled.
+*
+*       SET_DIV      : return the finally calculated frequency when clock is enabled,
+*				               return -1 when clock is disabled.
+*
+*       SET_PLL      : return the finally calculated frequency no matter clock is enabled or not.
+*
+*       SET_PLLDIV   : return the finally calculated frequency when clock is enabled,
+*				               return -1 when clock is disabled.
+* Caution :
+* 1. The final clock freq maybe an approximative value,
+*    equal to or less than the setting freq.
+* 2. SET_DIV and SET_PLLDIV commands which would set divisor register must use CLK_ENABLE command
+*    first to enable device clock.
+* 3. Due to default frequency may be extremely fast when clock is enabled. use SET_PLL command can
+*    set the frequency into a reasonable value, but don't need to enable clock first.
+*/
+#define PMC_TABLE 1
+int auto_pll_divisor(enum dev_id dev, enum clk_cmd cmd, int unit, int freq)
+{
+	int last_freq, divisor, en_count;
+	/*unsigned int t1 = 0, t2 = 0, t3;*/
+	/*if (mutexInit == 23456789) {
+		mutexInit = 0;
+		mutex_init(&clk_cnt_lock);
+	}*/
+
+	switch (cmd) {
+		case CLK_DISABLE:
+			if (dev < 128) {
+				//en_count = disable_dev_clk(dev);
+				en_count = enable_dev_clk(dev, 0);
+				return en_count;
+			} else {
+				printk(KERN_INFO"device has not clock enable register");
+				return -1;
+			}
+		case CLK_ENABLE:
+			if (dev < 128) {
+				en_count = enable_dev_clk(dev, 1);
+				return en_count;
+			} else {
+				printk(KERN_INFO"device has not clock enable register");
+				return -1;
+			}
+		case GET_FREQ:
+			#ifdef PMC_TABLE
+			/*t1 = wmt_read_oscr();*/
+			last_freq = get_freq_t(dev, &divisor);
+			/*t2 = wmt_read_oscr() - t1;
+			t1 = wmt_read_oscr();*/
+			#else
+			last_freq = get_freq(dev, &divisor);
+			#endif
+			/*t3 = wmt_read_oscr() - t1;
+			printk("*******************GF t3=%d tt2=%d min=%d\n", t3, t2, last_freq - last_freq1);*/
+			return last_freq;
+		case SET_DIV:
+			divisor = 0;
+			#ifdef PMC_TABLE
+			/*t1 = wmt_read_oscr();*/
+			last_freq = set_divisor_t(dev, unit, freq, &divisor);
+			/*t2 = wmt_read_oscr() - t1;
+			t1 = wmt_read_oscr();*/
+			#else
+			last_freq = set_divisor(dev, unit, freq, &divisor);
+			#endif
+			/*t3 = wmt_read_oscr() - t1;
+			printk("*******************SD t3=%d tt2=%d min=%d\n", t3, t2, last_freq - last_freq1);*/
+			return last_freq;
+		case SET_PLL:
+			divisor = 0;
+			#ifdef PMC_TABLE
+			/*t1 = wmt_read_oscr();*/
+			last_freq = set_pll_speed_t(dev, unit, freq, &divisor, 1);
+			/*t2 = wmt_read_oscr() - t1;
+			t1 = wmt_read_oscr();*/
+			#else
+			last_freq = set_pll_speed(dev, unit, freq, &divisor, 1);
+			#endif
+			/*t3 = wmt_read_oscr() - t1;
+			printk("*******************SP t3=%d tt2=%d min=%d\n", t3, t2, last_freq - last_freq1);*/
+			return last_freq;
+		case SET_PLLDIV:
+			divisor = 0;
+			#ifdef PMC_TABLE
+			/*t1 = wmt_read_oscr();*/
+			last_freq = set_pll_divisor_t(dev, unit, freq, &divisor);
+			/*t2 = wmt_read_oscr() - t1;
+			t1 = wmt_read_oscr();*/
+			#else
+			last_freq = set_pll_divisor(dev, unit, freq, &divisor);
+			#endif
+			/*t3 = wmt_read_oscr() - t1;
+			printk("******************SPD t3=%d tt2=%d min=%d\n", t3, t2, last_freq - last_freq1);*/
+			return last_freq;
+		case GET_CPUTIMER:
+			last_freq = get_freq_timer(dev, &divisor);
+			return last_freq;
+		default:
+		printk(KERN_INFO"clock cmd unknow");
+		/*last_freq = get_freq(dev, &divisor);
+		last_freq = set_divisor(dev, unit, freq, &divisor);
+		last_freq = set_pll_speed(dev, unit, freq, &divisor, 1);
+		last_freq = set_pll_divisor(dev, unit, freq, &divisor);*/
+		return -1;
+	}
+}
+EXPORT_SYMBOL(auto_pll_divisor);
+
+/**
+* freq = src_freq * 2 * (DIVF+1)/((DIVR+1)*(2^DIVQ))
+*
+* dev : Target device ID to be set the clock.
+* DIVF : Feedback divider value.
+* DIVR : Reference divider value.
+* DIVQ : Output divider value.
+* dev_div : Divisor belongs to each device, 0 means not changed.
+* return : The final clock freq, in Hz, will be returned when success,
+*            -1 means fail (waiting busy timeout).
+*
+* caution :
+* 1. src_freq/(DIVR+1) should great than or equal to 10,
+*/
+int manu_pll_divisor(enum dev_id dev, int DIVF, int DIVR, int DIVQ, int dev_div)
+{
+
+	unsigned int PLL, freq, pmc_clk_en, old_divisor, div;
+	int PLL_NO, div_addr_offs, j = 0, SD_MMC = 0;
+	
+	if ((dev == DEV_SDMMC0)||(dev == DEV_SDMMC1)||(dev == DEV_SDMMC2))
+		SD_MMC = 1;
+	
+	if (DIVF < 0 || DIVF > MAX_DF){
+		printk(KERN_INFO"DIVF is out of range 0 ~ %d", MAX_DF);
+		return -1;
+	}
+	if (DIVR < 0 || DIVR > MAX_DR){
+		printk(KERN_INFO"DIVR is out of range 0 ~ %d", MAX_DR);
+		return -1;
+	}
+	if (DIVQ < 0 || DIVQ > MAX_DQ){
+		printk(KERN_INFO"DIVQ is out of range 0 ~ %d", MAX_DQ);
+		return -1;
+	}
+	if ((1000/SRC_FREQ) > ((2*(DIVF+1))/(DIVR+1))) {
+		printk(KERN_INFO"((2*(DIVF+1))/(DIVR+1)) should great than (1000/SRC_FREQ)");
+		return -1;
+	}
+	if ((2000/SRC_FREQ) < ((2*(DIVF+1))/(DIVR+1))) {
+		printk(KERN_INFO"((2*(DIVF+1))/(DIVR+1)) should less than (2000/SRC_FREQ)");
+		return -1;
+	}
+
+	if (dev_div > ((SD_MMC == 1)?63:31)){
+		printk(KERN_INFO"divisor is out of range 0 ~ 31");
+		return -1;
+	}
+
+	PLL_NO = calc_pll_num(dev, &div_addr_offs);
+	if (PLL_NO == -1) {
+		printk(KERN_INFO"device not found");
+		return -1;
+	}
+	old_divisor = *(volatile unsigned char *)(PMC_BASE + div_addr_offs);
+
+	if (SD_MMC == 1 && (dev_div&32))
+		j = 1; /* sdmmc has a another divider = 64 */	
+	div = dev_div&0x1F;	
+	freq = (1000 * SRC_FREQ * 2 * (DIVF+1))/((DIVR+1)*(1<<DIVQ)*div*(j?128:1));
+	freq *= 1000;
+	//printk(KERN_INFO"DIVF%d, DIVR%d, DIVQ%d, dev_div%d, freq=%dkHz\n", DIVF, DIVR, DIVQ, dev_div, freq);
+
+	PLL = (DIVF<<16) + (DIVR<<8) + DIVQ;
+
+	/* if the clk of the device is not enable, then enable it */
+	if (dev < 128) {
+		pmc_clk_en = *(volatile unsigned int *)(PMC_CLK + 4*(dev/32));
+		if (!(pmc_clk_en & (1 << (dev - 32*(dev/32)))))
+			enable_dev_clk(dev, 1);
+	}
+
+	check_PLL_DIV_busy();
+	if (old_divisor < dev_div) {
+		*(volatile unsigned int *)(PMC_BASE + div_addr_offs)
+		= (j?32:0) + ((div == 32) ? 0 : div)/* + (div&1) ? (1<<8): 0*/;
+		check_PLL_DIV_busy();
+	}
+	*(volatile unsigned int *)(PMC_PLL + 4*PLL_NO) = PLL;
+	check_PLL_DIV_busy();
+	if (old_divisor > dev_div) {
+		*(volatile unsigned int *)(PMC_BASE + div_addr_offs)
+		= (j?32:0) + ((div == 32) ? 0 : div)/* + (div&1) ? (1<<8): 0*/;
+		check_PLL_DIV_busy();
+	}
+
+
+	//PLL = (j?32:0) + ((div == 32) ? 0 : div) /*+ (div&1) ? (1<<8): 0*/;
+	//printk(KERN_INFO"set divisor =0x%x, divider address=0x%x\n", PLL, (PMC_BASE + div_addr_offs));
+	return freq;
+}
+EXPORT_SYMBOL(manu_pll_divisor);
+
+
+unsigned int ftb[20] = {0xffffffff};
+int set_plla_divisor(struct plla_param *plla_env)
+{
+	int divisor = 0, ret = 0, i;
+	unsigned int plla_clk, arm_div, l2c_div, l2c_tag, l2c_data, axi_div;
+	unsigned int cpu_clk, index, /*old_arm_div,*/ nand_clk, tfreq, tb_index;
+	unsigned int drv_en, tbl_num, info, tmp;
+	int varlen = 512;
+	unsigned char buf[512] = {0};
+	char *ptr = NULL;
+	
+	plla_clk = plla_env->plla_clk;
+	arm_div = plla_env->arm_div;
+	l2c_div = plla_env->l2c_div;
+	l2c_tag = plla_env->l2c_tag_div;
+	l2c_data = plla_env->l2c_data_div;
+	axi_div = plla_env->axi_div;
+	tb_index = plla_env->tb_index;
+	
+	cpu_clk = get_freq_t(DEV_ARM, &divisor);
+	cpu_clk >>= 20;
+	//cpu_clk /= 1000000;
+	tfreq = plla_clk/arm_div;
+
+	info = *(volatile unsigned int *)(CPINFO);
+	if (ftb[0] == 0xffffffff) {
+		if ((info&0x8) == 0)
+			ret = wmt_getsyspara("wmt.cfadj.param", buf, &varlen);
+		else
+			ret = wmt_getsyspara("wmt.cflpadj.param", buf, &varlen);
+		if (ret) {
+			printk(KERN_INFO "Can not find uboot env wmt.cfadj.param\n");
+			ret = -ENODATA;
+			ftb[0] = 0;
+			goto no_adj;
+		}
+		sscanf(buf, "%x:%d:", &drv_en, &tbl_num);
+		if ((drv_en&1) == 0 || tbl_num == 0) {
+			ret = -ENODATA;
+			ftb[0] = 0;
+			goto no_adj;
+		}
+
+		ptr = buf;
+		if (tbl_num > 20)
+			tbl_num = 20;
+		fq_dbg("dvfs_adj_table:");
+		for (i = 0; i < tbl_num; i++) {
+			strsep(&ptr, "[");
+			sscanf(ptr, "%d][", &tmp);
+			ftb[i] = tmp;
+			fq_dbg("ftb[%d]=%d \n", i, ftb[i]);
+		}
+		fq_dbg("\n");
+	}
+	#ifdef DEBUG_PLLA
+	if (ftb[0] != 0)
+		tblpll = ftb[tb_index];
+	else
+		ret = 1;
+	//printk("tblpll = %d\n", tblpll);
+	#endif
+no_adj:
+	//printk("ret = %d\n", ret);
+	if (!ret) {
+		if (tfreq != tblpll) {
+			fr_rdn = (tfreq*15625)<<6;
+			tfreq = tblpll;
+			plla_clk = tfreq;
+			arm_div = 1;
+		} else
+			fr_rdn = 0;
+	} else {
+		#ifdef DEBUG_PLLA_FX
+		if (tfreq > (MAXPLL/1000000)) {
+			fr_rdn = tfreq*1000000;
+			tfreq = (MAXPLL/1000000);
+			plla_clk = tfreq;
+			arm_div = 1;
+		} else
+		#endif
+			fr_rdn = 0;
+	}
+
+	//printk("tfreq = %d\n", tfreq);
+	#if 0
+	if (cpu_clk > tfreq) {
+		//change cpu from faster to slower
+		old_arm_div = *(volatile unsigned int *)(PMC_BASE + 0x300);
+		index = set_pll_speed_t(DEV_ARM, 1, (plla_clk*1000)/old_arm_div, &divisor, 1);
+		if (index < 0)
+			return -1;
+
+		if (*(volatile unsigned char *)(PMC_BASE + 0x300) != arm_div) {
+			*(volatile unsigned int *)(PMC_BASE + 0x300) = arm_div;
+			check_PLL_DIV_busy();
+		}
+		if (*(volatile unsigned int *)(PMC_BASE + 0x30C) != l2c_div) {
+			*(volatile unsigned int *)(PMC_BASE + 0x30C) = l2c_div;
+			check_PLL_DIV_busy();
+		}
+		if (*(volatile unsigned int *)(PMC_BASE + 0x3F0) != l2c_tag) {
+			*(volatile unsigned int *)(PMC_BASE + 0x3F0) = l2c_tag;
+			check_PLL_DIV_busy();
+		}
+		if (*(volatile unsigned int *)(PMC_BASE + 0x3F4) != l2c_data) {
+			*(volatile unsigned int *)(PMC_BASE + 0x3F4) = l2c_data;
+			check_PLL_DIV_busy();
+		}
+		if (*(volatile unsigned int *)(PMC_BASE + 0x3B0) != axi_div) {
+			*(volatile unsigned int *)(PMC_BASE + 0x3B0) = axi_div;
+			check_PLL_DIV_busy();
+		}
+	} else 
+	#endif
+	{
+		//change cpu from slower to faster
+		nand_clk = get_freq_t(DEV_NAND, &divisor);
+		//if ((nand_clk*divisor) > plla_clk)
+			*(volatile unsigned int *)(PMC_BASE + 0x300) = 2;
+		/*else
+			*(volatile unsigned int *)(PMC_BASE + 0x300) = arm_div;*/
+		check_PLL_DIV_busy();
+		if (*(volatile unsigned int *)(PMC_BASE + 0x30C) != l2c_div) {
+			*(volatile unsigned int *)(PMC_BASE + 0x30C) = l2c_div;
+			check_PLL_DIV_busy();
+		}
+		if (*(volatile unsigned int *)(PMC_BASE + 0x3F0) != l2c_tag) {
+			*(volatile unsigned int *)(PMC_BASE + 0x3F0) = l2c_tag;
+			check_PLL_DIV_busy();
+		}
+		if (*(volatile unsigned int *)(PMC_BASE + 0x3F4) != l2c_data) {
+			*(volatile unsigned int *)(PMC_BASE + 0x3F4) = l2c_data;
+			check_PLL_DIV_busy();
+		}
+		if (*(volatile unsigned int *)(PMC_BASE + 0x3B0) != axi_div) {
+			*(volatile unsigned int *)(PMC_BASE + 0x3B0) = axi_div;
+			check_PLL_DIV_busy();
+		}
+		//if ((nand_clk*divisor) > plla_clk) {
+			index = set_pll_speed_t(DEV_ARM, 1, (plla_clk*1000)/2, &divisor, 1);
+			if (index < 0)
+				return -1;
+			index = index*2;
+			*(volatile unsigned int *)(PMC_BASE + 0x300) = arm_div;
+		/*} else {
+			index = set_pll_speed_t(DEV_ARM, 1, (plla_clk*1000)/arm_div, &divisor, 0);
+			if (index < 0)
+				return -1;
+		}*/
+	}
+
+	return index;
+}
+EXPORT_SYMBOL(set_plla_divisor);
+
+int wm_pmc_set(enum dev_id dev, enum power_cmd cmd)
+{
+	int retval = -1;
+	unsigned int temp = 0, base = 0, mali_val = 0;
+	unsigned int mali_off, mali_l2c_off = 0x00130620/*, mali_gp_off = 0x00130624*/;
+	mali_off = mali_l2c_off;
+
+	base = 0xfe000000;
+	temp = REG32_VAL(base + 0x00110110);
+	temp = ((temp >> 10)&0xC0)|(temp&0x3F);
+
+	if ((cmd == DEV_PWRON)) {
+		if ((temp & 0xC0) == 0x80) {//8700/8720 can't be power on
+			retval = -1;
+		} else {
+			while ((REG32_VAL(base + mali_off)&0xF0)!= 0
+			&& (REG32_VAL(base + mali_off)&0xF0)!= 0xF0)
+				;
+
+			mali_val = REG32_VAL(base + mali_off);
+			if (!((mali_val&0xF0)==0xF0))
+				REG32_VAL(base + mali_off) |= 1;
+
+			while ((REG32_VAL(base + mali_off)&0xF0)!= 0
+			&& (REG32_VAL(base + mali_off)&0xF0)!= 0xF0)
+				;
+			retval = 0;
+		}
+	} else if (cmd == DEV_PWROFF) {
+		if ((temp & 0xC3) == 0xC0) {//8710B0 can't power off after power on
+			retval = -1;
+		} else {
+			while ((REG32_VAL(base + mali_off)&0xF0)!= 0
+			&& (REG32_VAL(base + mali_off)&0xF0)!= 0xF0)
+				;
+			mali_val = REG32_VAL(base + mali_off);
+			if ((mali_val&0xF0))
+				REG32_VAL(base + mali_off) &= ~1;
+			while ((REG32_VAL(base + mali_off)&0xF0)!= 0
+			&& (REG32_VAL(base + mali_off)&0xF0)!= 0xF0)
+				;
+			retval = 0;
+		}
+	} else if (cmd == DEV_PWRSTS) {
+		while ((REG32_VAL(base + mali_off)&0xF0)!= 0
+		&& (REG32_VAL(base + mali_off)&0xF0)!= 0xF0)
+			;
+		temp = REG32_VAL(base + mali_off);
+		if (temp & 0xF0)
+			retval = 1;
+		else
+			retval = 0;
+	}
+	return retval;
+}
+
+
+
+#define LOADER_ADDR												0xffff0000
+#define HIBERNATION_ENTER_EXIT_CODE_BASE_ADDR	0xFFFFFFC0
+#define DO_POWER_SET							      (HIBERNATION_ENTER_EXIT_CODE_BASE_ADDR + 0x2C)
+#define CP15_C1_XPbit 23
+/*
+ * Function:
+ * int wmt_power_dev(enum dev_id dev, enum power_cmd cmd)
+ * This function is used to control/get WMT SoC specific device power state.
+ * 
+ * Parameter:
+ *  The available dev_id is DEV_MALI cmd used to control/get device power state. 
+ *  The available power_cmd are
+ *  - DEV_PWRON
+ *  - DEV_PWROFF
+ *  - DEV_PWRSTS
+ * 
+ * Return:
+ *  - As power_cmd are DEV_PWRON, DEV_PWROFF 0 indicates success. 
+ *    Negative value indicates failure as error code.
+ * 
+ *  - As power_cmd is DEV_PWRSTS
+ * 	 0 indicates the current device is power off.
+ * 	 1 indicates the current device is power on.
+ * 	 Negative value indicates failure as error code.
+ */
+extern int spi_read_status(int chip);
+int wmt_power_dev(enum dev_id dev, enum power_cmd cmd)
+{
+#if 0
+	static unsigned int base = 0;
+	unsigned int exec_at = (unsigned int)-1, temp = 0, bootdev;
+	int (*theKernel_power)(int from, enum dev_id dev, enum power_cmd cmd);
+	int en_count, retval = 0, rc = 0;
+	unsigned long flags;
+	bootdev = GPIO_STRAP_STATUS_VAL;
+
+	/*printk(KERN_INFO"entry dev_id=%d cmd=%d, boot_strap=%x,\n",dev, cmd, GPIO_STRAP_STATUS_VAL);*/
+	/*enble SF clock*/
+	if((bootdev & 0x4040) == 0)
+		en_count = enable_dev_clk(DEV_SF); //SF boot
+	else if((bootdev & 0x4040) == 4) 
+		en_count = enable_dev_clk(DEV_NAND);//NAND boot
+	else
+		en_count = enable_dev_clk(DEV_SDMMC0);//MMC boot
+	udelay(1);
+#ifdef CONFIG_MTD_WMT_SF	
+	rc = spi_read_status(0);
+	if (rc)
+		printk("wr c0 wait status ret=%d\n", rc);
+#endif	
+	/*rc = spi_read_status(1);
+	if (rc)
+		printk("wr c1 wait status ret=%d\n", rc);*/
+	
+	rc = 0;
+	/*jump to loader api to do something*/
+	if (base == 0)
+		base = (unsigned int)ioremap/*_nocache*/(LOADER_ADDR, 0x10000);
+	exec_at = base + (DO_POWER_SET - LOADER_ADDR);
+	theKernel_power = (int (*)(int from, enum dev_id dev, enum power_cmd cmd))exec_at;
+	/*temp = *(volatile unsigned int *)(0xFE110110);
+	printk(KERN_INFO"entry exec_at=0x%x chip_id=0x%x, clock enable=0x%x\n", exec_at,
+	((temp >> 10)&0xC0)|(temp&0x3F), *(volatile unsigned int *)(0xFE130250));*/
+	/*backup flags and disable irq*/
+	local_irq_save(flags);
+	/*enable subpage AP bits*/
+	asm volatile ("mrc p15, 0, %0, c1, c0, 0" : "=r" (temp));
+	//if (!(temp & (1<<CP15_C1_XPbit)))
+//		printk(KERN_INFO"1 xp(23) bits =0x%x cp15c0=0x%x\n", temp&(1<<CP15_C1_XPbit), temp);
+	if (temp & (1<<CP15_C1_XPbit)) {
+		rc = 1;
+		temp &= ~(1<<CP15_C1_XPbit);
+	  /*printk(KERN_INFO"2 xp(23) bits =0x%x \n", temp&(1<<CP15_C1_XPbit));*/
+		asm volatile ("mcr p15, 0, %0, c1, c0, 0" : : "r" (temp));
+	}
+	//retval = theKernel_power(4, DEV_MALI, cmd);
+	retval = wm_pmc_set(DEV_MALI, cmd);
+
+	if (rc == 1) {
+		/*disable subpage AP bits*/
+		asm volatile ("mrc p15, 0, %0, c1, c0, 0" : "=r" (temp));
+		/*printk(KERN_INFO"3 xp(23) bits =0x%x \n", temp&(1<<CP15_C1_XPbit));*/
+		temp |= (1<<CP15_C1_XPbit);
+		/*printk(KERN_INFO"4 xp(23) bits =0x%x \n", temp&(1<<CP15_C1_XPbit));*/
+		asm volatile ("mcr p15, 0, %0, c1, c0, 0" : : "r" (temp));
+	}
+	/*restore irq flags*/
+	local_irq_restore(flags);
+
+	/*iounmap((void *)base);*/
+	/*printk(KERN_INFO"entry base=0x%x exec_at=0x%x clock enable =0x%x\n",
+	base, exec_at, *(volatile unsigned int *)(0xFE130250));*/
+
+	/*disable SF clock*/
+	if((bootdev & 0x4040) == 0)
+		en_count = disable_dev_clk(DEV_SF); //SF boot
+	else if((bootdev & 0x4040) == 4) 
+		en_count = disable_dev_clk(DEV_NAND);//NAND boot
+	else
+		en_count = disable_dev_clk(DEV_SDMMC0);//MMC boot
+
+	/*printk(KERN_INFO"exit!!ret = (%d)\n",retval);*/
+#endif
+	return 0;//retval;
+}
+EXPORT_SYMBOL(wmt_power_dev);
+
+/*struct pll_map pllmapAll[] = {
+{126,		0x00140003}, {126,	0x00290103}, {126, 	0x00140102},
+{129,		0x002A0103},
+{132,		0x00150003}, {132,	0x002B0103}, {132, 	0x00150102}, {132, 	0x000A0101}, {132, 	0x000A0002},
+{135,		0x002C0103},
+{138,		0x00160003}, {138,	0x002D0103}, {138, 	0x00160102},
+{141,		0x002E0103},
+{144,		0x00170003}, {144,	0x002F0103}, {144,	0x00020000}, {144, 	0x00170102}, {144, 	0x000B0101}, {144,	0x00050100}, {144, 	0x000B0002}, {144, 	0x00050001},
+{147,		0x00300103},
+{150,		0x00180003}, {150,	0x00310103}, {150, 	0x00180102},
+{153,		0x00320103},
+{156,		0x00190003}, {156,	0x00330103}, {156, 	0x000C0101}, {156, 	0x000C0002},
+{159,		0x00340103},
+{162,		0x001A0003}, {162,	0x00350103}, {162, 	0x001A0102},
+{165,		0x00360103}, {156, 	0x00190102},
+{168,		0x001B0003}, {168,	0x00370103}, {168, 	0x001B0102}, {168, 	0x000D0101}, {168,	0x00060100}, {168, 	0x000D0002}, {168, 	0x00060001},
+{171,		0x00380103},
+{174,		0x001C0003}, {174,	0x00390103}, {174, 	0x001C0102},
+{177,		0x003A0103},
+{180,		0x001D0003}, {180,	0x003B0103}, {180, 	0x001D0102}, {180, 	0x000E0101}, {180, 	0x000E0002},
+{183,		0x003C0103},
+{186,		0x001E0003}, {186, 	0x001E0102}, {186, 	0x003D0103},
+{189,		0x003E0103},
+{192,		0x001F0003}, {192,	0x00030000}, {192, 	0x003F0103}, {192, 	0x001F0102}, {192, 	0x000F0101}, {192,	0x00070100}, {192, 	0x000F0002}, {192, 	0x00070001},
+{195,		0x00400103},
+{198,		0x00200003}, {198, 	0x00200102},
+{198,		0x00410103},
+{201,		0x00420103},
+{204,		0x00210003}, {204,	0x00430103}, {204, 	0x00210102}, {204, 	0x00100101}, {204, 	0x00100002},
+{207,		0x00440103},
+{210,		0x00220003}, {210,	0x00450103}, {210, 	0x00220102},
+{213,		0x00460103},
+{216,		0x00230003}, {216,	0x00470103}, {216, 	0x00230102}, {216, 	0x00110101}, {216, 	0x00080001}, {216,	0x00080100}, {216, 	0x00110002},
+{219,		0x00480103},
+{222,		0x00240003}, {222, 	0x00240102},
+{222,		0x00490103},
+{225,		0x004A0103},
+{228,		0x004B0103}, {228, 	0x00250102}, {228, 	0x00120101}, {228,	0x00250003}, {228, 	0x00120002},
+{231,		0x004C0103},
+{234,		0x00260003}, {234, 	0x00260102},
+{234,		0x004D0103},
+{237,		0x004E0103},
+{240,		0x00270003}, {240, 	0x00270102}, {240,	0x004F0103}, {240,	0x00040000}, {240, 	0x00130101}, {240, 	0x00090001}, {240,	0x00090100}, {240, 	0x00130002},
+{243,		0x00500103},
+{246,		0x00280003}, {246, 	0x00280102},
+{246,		0x00510103},
+{249,		0x00520103},
+{252,		0x00290003}, {252,	0x00530103}, {252, 	0x00290102}, {252, 	0x00140002}, {252, 	0x00140101},
+{258, 	0x002A0102},
+{264, 	0x00150002}, {264, 	0x002B0102}, {264, 	0x00150101}, {264, 	0x000A0001}, {264,	0x000A0100},
+{270, 	0x002C0102},
+{276, 	0x00160002}, {276, 	0x002D0102}, {276, 	0x00160101},
+{282, 	0x002E0102},
+{288, 	0x00170002}, {288, 	0x002F0102}, {288,	0x00050000}, {288, 	0x000B0001}, {288, 	0x00170101}, {288,	0x000B0100},
+{294, 	0x00300102},
+{300, 	0x00180002}, {300, 	0x00310102}, {300, 	0x00180101},
+{306, 	0x00320102},
+{312, 	0x00190002}, {312, 	0x00330102}, {312, 	0x000C0001}, {312, 	0x00190101}, {312,	0x000C0100},
+{318, 	0x00340102},
+{324, 	0x001A0002}, {324, 	0x00350102}, {324, 	0x001A0101},
+{330, 	0x00360102},
+{336, 	0x001B0002}, {336, 	0x00370102}, {336,	0x00060000}, {336, 	0x000D0001}, {336, 0x001B0101}, {336,	0x000D0100},
+{342, 	0x00380102},
+{348, 	0x001C0002}, {348, 	0x00390102}, {384,	0x00070000}, {348, 	0x001C0101}, {384,	0x000F0100}, {384, 	0x000F0001},
+{354, 	0x003A0102},
+{360, 	0x001D0002}, {360, 	0x003B0102}, {360, 	0x000E0001}, {360, 	0x001D0101}, {360,	0x000E0100},
+{366, 	0x003C0102},
+{372, 	0x001E0002}, {372, 	0x003D0102}, {372, 	0x001E0101},
+{378, 	0x003E0102},
+{384, 	0x001F0002}, {384, 	0x003F0102}, {384, 	0x001F0101},
+{390, 	0x00400102},
+{396, 	0x00200002}, {396, 	0x00410102}, {396, 	0x00200101},
+{402, 	0x00420102},
+{408, 	0x00210002}, {408, 	0x00430102}, {408, 	0x00210101}, {408,	0x00100100}, {408, 0x00100001},
+{414, 	0x00440102},
+{420, 	0x00220002}, {420, 	0x00450102}, {420, 	0x00220101},
+{426, 	0x00460102},
+{432, 	0x00230002}, {432, 	0x00470102}, {432, 	0x00230101}, {432,	0x00080000}, {432, 0x00110001}, {432, 0x00110100},
+{438, 	0x00480102}, {480, 	0x00130001}, {480,	0x00130100}, {480,	0x00090000},
+{444, 	0x00240002}, {444, 	0x00490102}, {444, 	0x00240101},
+{450, 	0x004A0102},
+{456, 	0x00250002}, {456, 	0x004B0102}, {456, 	0x00250101}, {456,	0x00120100}, {456, 0x00120001},
+{462, 	0x004C0102},
+{468, 	0x00260002}, {468, 	0x004D0102}, {468, 	0x00260101},
+{474, 	0x004E0102},
+{480, 	0x00270002}, {480, 	0x004F0102}, {480, 	0x00270101},
+{486, 	0x00500102},
+{492, 	0x00280002}, {492, 	0x00510102}, {492, 	0x00280101},
+{498, 	0x00520102},
+{504, 	0x00140001}, {504, 	0x00290002}, {504,	0x00530102}, {504, 	0x00290101}, {504, 0x00140100},
+{516, 	0x002A0101},
+{528, 	0x00150001}, {528, 	0x002B0101}, {528,	0x00150100}, {528,	0x000A0000},
+{540, 	0x002C0101},
+{552, 	0x00160001}, {552, 	0x002D0101}, {552,	0x00160100},
+{564, 	0x002E0101},
+{576, 	0x00170001}, {576, 	0x002F0101}, {576,	0x00170100}, {576,	0x000B0000},
+{588, 	0x00300101},
+{600, 	0x00180001}, {600, 	0x00310101}, {600,	0x00180100},
+{612, 	0x00320101},
+{624, 	0x00190001}, {624, 	0x00330101}, {624,	0x000C0000}, {624,	0x00190100},
+{636, 	0x00340101},
+{648, 	0x001A0001}, {648, 	0x00350101}, {648,	0x001A0100},
+{660, 	0x00360101},
+{672, 	0x001B0001}, {672, 	0x00370101}, {672,	0x000D0000}, {672,	0x001B0100},
+{684, 	0x00380101},
+{696, 	0x001C0001}, {696, 	0x00390101}, {696,	0x001C0100},
+{708, 	0x003A0101},
+{720, 	0x001D0001}, {720, 	0x003B0101}, {720,	0x000E0000}, {720,	0x001D0100},
+{732, 	0x003C0101},
+{744, 	0x001E0001}, {744, 	0x003D0101}, {744,	0x001E0100},
+{756, 	0x003E0101},
+{768, 	0x001F0001}, {768, 	0x003F0101}, {768,	0x000F0000}, {768,	0x001F0100},
+{780, 	0x00400101},
+{792, 	0x00200001}, {792, 	0x00410101}, {792,	0x00200100},
+{804, 	0x00420101},
+{816, 	0x00210001}, {816, 	0x00430101}, {816,	0x00100000}, {816,	0x00210100},
+{828, 	0x00440101},
+{840, 	0x00220001}, {840, 	0x00450101}, {840,	0x00220100},
+{852, 	0x00460101},
+{864, 	0x00230001}, {864, 	0x00470101}, {864,	0x00230100}, {864,	0x00110000},
+{876, 	0x00480101},
+{888, 	0x00240001}, {888, 	0x00490101}, {888,	0x00240100},
+{900, 	0x004A0101},
+{912, 	0x00250001}, {912, 	0x004B0101}, {912,	0x00120000}, {912,	0x00250100},
+{924, 	0x004C0101},
+{936, 	0x004D0101}, {936,	0x00260100},
+{948, 	0x004E0101},
+{960, 	0x00270001}, {960, 	0x004F0101}, {960,	0x00270100}, {960,	0x00130000},
+{972, 	0x00500101},
+{984, 	0x00280001}, {984, 	0x00510101}, {984,	0x00280100},
+{936, 	0x00260001}, {996, 	0x00520101},
+{1008,	0x00290001}, {1008,	0x00530101}, {1008,	0x00290100}, {1008,	0x00140000},
+{1032,	0x002A0100},
+{1056,	0x00150000}, {1056,	0x002B0100},
+{1080,	0x002C0100},
+{1104,	0x00160000}, {1104,	0x002D0100},
+{1128,	0x002E0100},
+{1152,	0x00170000}, {1152,	0x002F0100},
+{1176,	0x00300100},
+{1200,	0x00180000}, {1200,	0x00310100},
+{1224,	0x00320100},
+{1248,	0x00190000}, {1248,	0x00330100},
+{1272,	0x00340100},
+{1296,	0x001A0000}, {1296,	0x00350100},
+{1320,	0x00360100},
+{1344,	0x001B0000}, {1344,	0x00370100},
+{1368,	0x00380100},
+{1392,	0x001C0000}, {1392,	0x00390100},
+{1416,	0x003A0100},
+{1440,	0x001D0000}, {1440,	0x003B0100},
+{1464,	0x003C0100},
+{1488,	0x001E0000}, {1488,	0x003D0100},
+{1512,	0x003E0100},
+{1536,	0x001F0000}, {1536,	0x003F0100},
+{1560,	0x00400100},
+{1584,	0x00200000}, {1584,	0x00410100},
+{1608,	0x00420100},
+{1632,	0x00210000}, {1632,	0x00430100},
+{1656,	0x00440100},
+{1680,	0x00220000}, {1680,	0x00450100},
+{1704,	0x00460100},
+{1728,	0x00230000}, {1728,	0x00470100},
+{1752,	0x00480100},
+{1776,	0x00240000}, {1776,	0x00490100},
+{1800,	0x004A0100},
+{1824,	0x004B0100}, {1824,	0x00250000},
+{1848,	0x004C0100},
+{1872,	0x00260000}, {1872,	0x004D0100},
+{1896,	0x004E0100},
+{1920,	0x00270000}, {1920,	0x004F0100},
+{1944,	0x00500100},
+{1968,	0x00280000}, {1968,	0x00510100},
+{1992,	0x00520100},
+{2016,	0x00290000}, {2016,	0x00530100}
+};*/
+void wmt_set_DIV(enum dev_id dev,int div)
+{
+	int PLL_NO, div_addr_offs;
+
+	PLL_NO = calc_pll_num(dev, &div_addr_offs);
+	check_PLL_DIV_busy();
+	if (dev == DEV_WMTNA)
+		*(volatile unsigned int *)(PMC_BASE + div_addr_offs) =	(0x200 | ((div == 32) ? 0 : div));
+	else
+		*(volatile unsigned int *)(PMC_BASE + div_addr_offs)
+		=	((dev == DEV_SDTV)?0x10000:0) + ((div == 32) ? 0 : div);
+	check_PLL_DIV_busy();
+}
+
+extern int vpp_parse_param(char *buf, unsigned int *param,
+					int cnt, unsigned int hex_mask);
+
+#define WMT_VDD_CONFIG
+struct workqueue_struct *mmfreq_workqueue = 0;
+struct work_struct mmfreq_work;
+static int div_table[3][7];
+static int table_num;
+unsigned int mc5_08_default;
+unsigned int mc5_18_default;
+unsigned int mc5_20_default;
+
+void wmt_resume_mmfreq(void)
+{
+	int current_voltage = 0;
+
+	if (re) {
+		current_voltage = regulator_get_voltage(re);
+		regulator_set_voltage(re, current_voltage, current_voltage);
+	}
+	mmfreq_cur_num = ~0;
+}
+
+void wmt_suspend_mmfreq(void)
+{
+	if (!mmfreq_workqueue)
+		return;
+
+	flush_workqueue(mmfreq_workqueue);
+}
+
+void wmt_do_mmfreq(struct work_struct *ptr)
+{
+	static int init;
+
+	int num = mmfreq_num;
+	int dev_cnt;
+
+	wmt_clk_mutex_lock(1);
+	dev_cnt = dev_en_count[MMFREQ_VD];
+	wmt_clk_mutex_lock(0);
+
+
+//	printk("%s %d --> %d,%d\n", __FUNCTION__, mmfreq_cur_num, num,dev_cnt);
+
+	if (init == 0) {
+#ifdef WMT_VDD_CONFIG
+		re = regulator_get(NULL, "wmt_vdd");
+#endif
+		init = 1;
+	}
+
+	if (dev_cnt)
+		return;
+
+	if (num > table_num)
+		return;
+
+	if (num == mmfreq_cur_num)
+		return;
+
+	if (mmfreq_debug) {
+#ifdef WMT_VDD_CONFIG
+		printk("mmfreq %d --> %d,re 0x%x\n",mmfreq_cur_num,num,(int)re);
+#endif
+		printk("vol %d,mali %d,vpp %d,vdu %d,vduna %d,cmn %d,cmnna %d\n",
+			div_table[num][0], div_table[num][1],
+			div_table[num][2], div_table[num][3],
+			div_table[num][4], div_table[num][5],
+			div_table[num][6]);
+	}
+#ifdef WMT_VDD_CONFIG
+	if (mmfreq_cur_num < num) { /* lo to hi */
+		regulator_set_voltage(re, div_table[num][0]*1000, div_table[num][0]*1000);
+	}
+#endif
+	wmt_set_DIV(DEV_MALI,div_table[num][1]);
+	wmt_set_DIV(DEV_VPP,div_table[num][2]);
+	wmt_set_DIV(DEV_WMTVDU,div_table[num][3]);	
+	wmt_set_DIV(DEV_WMTNA,div_table[num][4]);
+	wmt_set_DIV(DEV_CNMVDU,div_table[num][5]);
+	wmt_set_DIV(DEV_CNMNA,div_table[num][6]);
+#ifdef WMT_VDD_CONFIG
+	if (mmfreq_cur_num > num) { /* hi to lo */
+		regulator_set_voltage(re, div_table[num][0]*1000, div_table[num][0]*1000);
+	}
+#endif
+	mmfreq_cur_num = num;
+	printk("vol %d,mali %d,vpp %d,vdu %d,vduna %d,cmn %d,cmnna %d,end wmt_do_mmfreq\n",
+		div_table[num][0], div_table[num][1],
+		div_table[num][2], div_table[num][3],
+		div_table[num][4], div_table[num][5],
+		div_table[num][6]);
+}
+
+void wmt_set_mmfreq(int num)
+{
+	(*(volatile unsigned int *)(MEMORY_CTRL_V4_CFG_BASE_ADDR + 0x08)) =
+		(num) ? 0x00f80000 : mc5_08_default;
+	(*(volatile unsigned int *)(MEMORY_CTRL_V4_CFG_BASE_ADDR + 0x18)) =
+		(num) ? 0x00fa0000 : mc5_18_default;
+	(*(volatile unsigned int *)(MEMORY_CTRL_V4_CFG_BASE_ADDR + 0x20)) =
+		(num) ? 0x00020505 : mc5_20_default;
+
+	if (!mmfreq_workqueue)
+		return;
+	
+//	printk("%s %d --> %d\n", __FUNCTION__, mmfreq_cur_num, num);
+	mmfreq_num = num;
+	if (mmfreq_cur_num == num)
+		return;
+
+	flush_workqueue(mmfreq_workqueue);
+	queue_work(mmfreq_workqueue, &mmfreq_work);
+}
+
+void wmt_enable_mmfreq(enum wmt_mmfreq_type type, int enable)
+{	
+	wmt_clk_mutex_lock(1);
+	mmfreq_type_mask = (enable) ?
+		(mmfreq_type_mask | type) : (mmfreq_type_mask & ~type);
+	wmt_clk_mutex_lock(0);
+	wmt_set_mmfreq((mmfreq_type_mask) ? 1 : 0);
+}
+
+int wmt_mmfreq_init(void)
+{
+	char buf[100];
+	int varlen = 100;
+	int parm[21];
+	int i;
+	unsigned int info;
+
+	mc5_08_default =
+	(*(volatile unsigned int *)(MEMORY_CTRL_V4_CFG_BASE_ADDR + 0x08));
+	mc5_18_default =
+	(*(volatile unsigned int *)(MEMORY_CTRL_V4_CFG_BASE_ADDR + 0x18));
+	mc5_20_default =
+	(*(volatile unsigned int *)(MEMORY_CTRL_V4_CFG_BASE_ADDR + 0x20));
+
+	info = *(volatile unsigned int *)(CPINFO);
+	if (info & 0x8)
+		i = wmt_getsyspara("wmt.mmfreqlp.param", buf, &varlen);
+	else
+		i = wmt_getsyspara("wmt.mmfreq.param", buf, &varlen);
+
+	if (i)
+		return 0;
+
+	table_num = vpp_parse_param(buf, (unsigned int *)parm, 21, 0x1);
+	if (parm[0] & 0x1) {
+		mmfreq_debug = (parm[0] & 0x10) ? 1 : 0;
+#if 0
+		for (i = 0; i < table_num; i++ ) {
+			printk("%d:%d,",i,parm[i]);
+		}
+		printk("\n");
+#endif
+		table_num = (table_num - 3 + 1) / 9;
+		printk("mmfreq.parm en 0x%x,sr %d,num %d,%d\n",parm[0],parm[1],parm[2],table_num);
+		for (i=0; i<table_num; i++) {
+			div_table[i][0] = parm[9 * i + 4];
+			div_table[i][1] = parm[9 * i + 5];
+			div_table[i][2] = parm[9 * i + 6];
+			div_table[i][3] = parm[9 * i + 7];
+			div_table[i][4] = parm[9 * i + 8];
+			div_table[i][5] = parm[9 * i + 9];
+			div_table[i][6] = parm[9 * i + 10];
+			printk("vol %d,mali %d,vpp %d,vdu %d,vduna %d,cmn %d,cmnna %d\n",
+				div_table[i][0], div_table[i][1],
+				div_table[i][2], div_table[i][3],
+				div_table[i][4], div_table[i][5],
+				div_table[i][6]);
+		}
+	}
+	else {
+		table_num = 0;
+		printk("mmfreq disable\n");
+	}
+
+	if (table_num >= 2) {
+		mmfreq_workqueue = create_singlethread_workqueue("mmfreq_wq");
+		if (!mmfreq_workqueue)
+			return -1;
+		INIT_WORK(&mmfreq_work,wmt_do_mmfreq);
+	}
+	return 0;
+}
+module_init(wmt_mmfreq_init);
+
diff --git a/arch/arm/mach-wmt/wmt_cpufreq.c b/arch/arm/mach-wmt/wmt_cpufreq.c
new file mode 100644
index 00000000..f5046f40
--- /dev/null
+++ b/arch/arm/mach-wmt/wmt_cpufreq.c
@@ -0,0 +1,752 @@
+/*++
+	linux/arch/arm/mach-wmt/wmt_cpufreq.c
+
+	Copyright (c) 2013  WonderMedia Technologies, Inc.
+
+	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, see <http://www.gnu.org/licenses/>.
+
+	WonderMedia Technologies, Inc.
+	10F, 529, Chung-Cheng Road, Hsin-Tien, Taipei 231, R.O.C.
+--*/
+
+#include <linux/kernel.h>
+#include <linux/types.h>
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/module.h>
+#include <linux/cpufreq.h>
+#include <linux/delay.h>
+#include <linux/reboot.h>
+#include <linux/syscalls.h>
+#include <linux/suspend.h>
+#include <asm/smp_plat.h>
+#include <asm/cpu.h>
+#include <linux/regulator/consumer.h>
+
+#include <mach/wmt_env.h>
+#include <mach/hardware.h>
+
+/*
+#define DEBUG
+*/
+#ifdef DEBUG
+static int dbg_mask = 1;
+module_param(dbg_mask, int, S_IRUGO | S_IWUSR);
+#define fq_dbg(fmt, args...) \
+	do {\
+		if (dbg_mask) \
+			printk(KERN_ERR "[%s]_%d_%d: " fmt, __func__ , __LINE__, smp_processor_id(), ## args);\
+	} while (0)
+#define fq_trace() \
+	do {\
+		if (dbg_mask) \
+			printk(KERN_ERR "trace in %s %d\n", __func__, __LINE__);\
+	} while (0)
+#else
+#define fq_dbg(fmt, args...)
+#define fq_trace()
+#endif
+
+#define DVFS_TABLE_NUM_MAX  20
+#define DVFS_TABLE_NUM_MIN  2
+#define PMC_BASE            PM_CTRL_BASE_ADDR
+#define PLLA_FREQ_KHZ       (24 * 1000)
+#define PMC_PLLA            (PM_CTRL_BASE_ADDR + 0x200)
+#define ARM_DIV_OFFSET      (PM_CTRL_BASE_ADDR + 0x300)
+#define MILLISEC_TO_MICROSEC  1000
+
+struct wmt_dvfs_table {
+	unsigned int freq;
+	unsigned int vol;
+	unsigned int l2c_div;
+	unsigned int l2c_tag;
+	unsigned int l2c_data;
+	unsigned int axi;
+	int index;
+	struct list_head node;
+};
+
+struct wmt_dvfs_driver_data {
+	unsigned int tbl_num;
+	unsigned int sample_rate;
+	struct list_head wmt_dvfs_list;
+	struct wmt_dvfs_table *dvfs_table;
+	struct cpufreq_frequency_table *freq_table;
+};
+static struct wmt_dvfs_driver_data wmt_dvfs_drvdata;
+static struct regulator *re;
+static unsigned int use_regulator = 0;
+
+char	use_dvfs;/*flag for read env varalbe*/
+static char	use_dvfs_debug;
+static int wmt_dvfs_running;/*flag for reboot disable dvfs*/
+static DEFINE_SEMAPHORE(wmt_cpufreq_sem);
+
+/* register a reboot notifier, invoked in kernel_restart_prepare()
+ * or kernel_shutdown_prepare() */
+static int wmt_dvfs_reboot(struct notifier_block *, unsigned long, void *);
+static struct notifier_block wmt_reboot_notifier = {
+	.notifier_call = wmt_dvfs_reboot,
+};
+
+static int wmt_suspend_target(unsigned target, unsigned relation, unsigned is_suspend);
+static int wmt_dvfs_reboot(struct notifier_block *nb, unsigned long event, void *unused)
+{
+	unsigned target = 0xFFFFFFFF;
+	struct cpufreq_policy *policy = cpufreq_cpu_get(0); /* boot CPU */
+
+	/* find the highest freq in table */
+	if (policy)
+		target = policy->max;
+
+	/* change freq to highest to make sure reboot use the max volatage */
+	wmt_suspend_target(target, CPUFREQ_RELATION_H, 0);
+
+	return NOTIFY_OK;
+}
+
+static int wmt_init_cpufreq_table(struct wmt_dvfs_driver_data *drv_data)
+{
+	int i = 0;
+	struct wmt_dvfs_table *dvfs_tbl = NULL;
+	struct cpufreq_frequency_table *freq_tbl = NULL;
+
+	freq_tbl = kzalloc(sizeof(struct cpufreq_frequency_table) * (drv_data->tbl_num + 1),
+				GFP_KERNEL);
+	if (freq_tbl == NULL) {
+		printk(KERN_ERR "%s: failed to allocate frequency table\n", __func__);
+		return -ENOMEM;
+	}
+
+	drv_data->freq_table = freq_tbl;
+	list_for_each_entry(dvfs_tbl, &drv_data->wmt_dvfs_list, node) {
+		fq_dbg("freq_table[%d]:freq->%dKhz", i, dvfs_tbl->freq);
+		freq_tbl[i].index = i;
+		freq_tbl[i].frequency = dvfs_tbl->freq;
+		i++;
+	}
+	/* the last element must be initialized to CPUFREQ_TABLE_END */
+	freq_tbl[i].index = i;
+	freq_tbl[i].frequency = CPUFREQ_TABLE_END;
+
+	return 0;
+}
+
+/* wmt_getspeed return unit is Khz */
+static unsigned int wmt_getspeed(unsigned int cpu)
+{
+	int freq = 0;
+
+	if (cpu >= NR_CPUS)
+		return 0;
+
+	freq = auto_pll_divisor(DEV_ARM, GET_FREQ, 0, 0) / 1000;
+
+	if (freq < 0)
+		freq = 0;
+
+	return freq;
+}
+
+static struct wmt_dvfs_table *find_freq_ceil(unsigned int *target)
+{
+	struct wmt_dvfs_table *dvfs_tbl = NULL;
+	unsigned int freq = *target;
+
+	list_for_each_entry(dvfs_tbl, &wmt_dvfs_drvdata.wmt_dvfs_list, node) {
+		if (dvfs_tbl->freq >= freq) {
+			*target = dvfs_tbl->freq;
+			return dvfs_tbl;
+		}
+	}
+
+	return NULL;
+}
+
+static struct wmt_dvfs_table *find_freq_floor(unsigned int *target)
+{
+	struct wmt_dvfs_table *dvfs_tbl = NULL;
+	unsigned int freq = *target;
+
+	list_for_each_entry_reverse(dvfs_tbl, &wmt_dvfs_drvdata.wmt_dvfs_list, node) {
+		if (dvfs_tbl->freq <= freq) {
+			*target = dvfs_tbl->freq;
+			return dvfs_tbl;
+		}
+	}
+
+	return NULL;
+}
+
+static struct wmt_dvfs_table *
+wmt_recalc_target_freq(unsigned *target_freq, unsigned relation)
+{
+	struct wmt_dvfs_table *dvfs_tbl = NULL;
+
+	if (!target_freq)
+		return NULL;
+
+	switch (relation) {
+	case CPUFREQ_RELATION_L:
+		/* Try to select a new_freq higher than or equal target_freq */
+		dvfs_tbl = find_freq_ceil(target_freq);
+		break;
+	case CPUFREQ_RELATION_H:
+		/* Try to select a new_freq lower than or equal target_freq */
+		dvfs_tbl = find_freq_floor(target_freq);
+		break;
+	default:
+		dvfs_tbl = NULL;
+		break;
+	}
+
+	return dvfs_tbl;
+}
+
+/* this is a debug func, for checking pmc divf, divr, divq value */
+static int get_arm_plla_param(void)
+{
+#ifdef DEBUG
+	unsigned ft, df, dr, dq, div, tmp;
+
+	tmp =  *(volatile unsigned int *)PMC_PLLA;
+	fq_dbg("PMC PLLA REG IS 0x%08x\n", tmp);
+
+	ft = (tmp >> 24) & 0x03; /*bit24 ~ bit26*/
+	df = (tmp >> 16) & 0xFF; /*bit16 ~ bit23*/
+	dr = (tmp >>  8) & 0x1F; /*bit8  ~ bit12*/
+	dq = tmp & 0x03;
+
+	tmp = *(volatile unsigned int *)ARM_DIV_OFFSET;
+	div = tmp & 0x1F;
+
+	fq_dbg("ft:%d, df:%d, dr:%d, dq:%d, div:%d\n", ft, df, dr, dq, div);
+#endif
+	return 0;
+}
+
+static int wmt_change_plla_table(struct wmt_dvfs_table *plla_table, unsigned relation)
+{
+	int ret = 0;
+	int ret_vol = 0;
+	struct plla_param plla_env;
+
+	plla_env.plla_clk = (plla_table->freq / 1000);
+	plla_env.arm_div = 1;
+	plla_env.l2c_div = plla_table->l2c_div;
+	plla_env.l2c_tag_div = plla_table->l2c_tag;
+	plla_env.l2c_data_div = plla_table->l2c_data;
+	plla_env.axi_div = plla_table->axi;
+	plla_env.tb_index = plla_table->index;
+
+	switch (relation) {
+	case CPUFREQ_RELATION_L:
+		if (use_dvfs_debug)
+			printk(KERN_INFO "change to L %dKhz\n", plla_table->freq);
+		ret = set_plla_divisor(&plla_env);
+		if (plla_table->vol) {
+			if (use_dvfs_debug)
+				printk(KERN_INFO "pllal_table = %d\n", plla_table->vol*1000);
+			if (use_regulator) {
+				ret_vol = regulator_set_voltage(re, plla_table->vol*1000, plla_table->vol*1000);
+				if (ret_vol < 0)
+					printk(KERN_INFO "xxx set vol fail %d\n", ret_vol);
+			}
+		}
+		break;
+	case CPUFREQ_RELATION_H:
+		if ((use_regulator == 1) && (regulator_is_enabled(re) < 0)) {
+			if (use_dvfs_debug)
+				printk(KERN_INFO "xxx regulator is disabled\n");
+			ret = -EINVAL;
+		} else {
+			if (plla_table->vol) {
+				if (use_dvfs_debug)
+					printk(KERN_INFO "pllah_table = %d\n", plla_table->vol*1000);
+				if (use_regulator)
+					ret = regulator_set_voltage(re, plla_table->vol*1000, plla_table->vol*1000);			
+			}
+			if (ret >= 0) {
+				if (use_dvfs_debug)
+					printk(KERN_INFO "change to H %dKhz\n", plla_table->freq);
+				ret = set_plla_divisor(&plla_env);
+			} else
+				printk(KERN_INFO "xxx set vol fail %d\n", ret);
+		}
+		break;
+	default:
+		break;
+	}
+
+	return ret;
+}
+
+/* target is calculated by cpufreq governor, unit Khz */
+static int freq_save = 0; // unit Khz
+static int wmt_target(struct cpufreq_policy *, unsigned, unsigned);
+static int wmt_suspend_target(unsigned target, unsigned relation, unsigned is_suspend)
+{
+	unsigned int cur_freq = wmt_getspeed(0);
+	int voltage = 0;
+	
+	down(&wmt_cpufreq_sem);
+
+	/*
+	 * Set to the highest voltage before suspend/reboot
+	 */
+	if (use_regulator) {
+		if (use_dvfs_debug)
+			printk("Set to Max. Voltage: %dmV\n", wmt_dvfs_drvdata.dvfs_table[(wmt_dvfs_drvdata.tbl_num - 1)].vol);
+		regulator_set_voltage(re, wmt_dvfs_drvdata.dvfs_table[(wmt_dvfs_drvdata.tbl_num - 1)].vol * 1000,
+					wmt_dvfs_drvdata.dvfs_table[(wmt_dvfs_drvdata.tbl_num - 1)].vol * 1000);
+		voltage = regulator_get_voltage(re);
+		printk("Current voltage = %d\n", voltage);
+	}
+	/* for some governor, like userspace, performance, powersaving,
+	 * need change frequency to pre-suspend when resume */
+	wmt_dvfs_running = 0;
+	freq_save = cur_freq;
+	up(&wmt_cpufreq_sem);
+
+	return 0;
+}
+
+/*
+ * Note that loops_per_jiffy is not updated on SMP systems in
+ * cpufreq driver. So, update the per-CPU loops_per_jiffy value
+ * on frequency transition. We need to update all dependent CPUs.
+ */
+#ifdef CONFIG_SMP
+struct lpj_info {
+	unsigned long	ref;
+	unsigned int	freq;
+};
+static DEFINE_PER_CPU(struct lpj_info, lpj_ref);
+static struct lpj_info global_lpj_ref;
+
+static void
+wmt_update_lpj(struct cpufreq_policy *policy, struct cpufreq_freqs freqs)
+{
+	unsigned int i = 0;
+
+	for_each_cpu(i, policy->cpus) {
+		struct lpj_info *lpj = &per_cpu(lpj_ref, i);
+		if (!lpj->freq) {
+			lpj->ref = per_cpu(cpu_data, i).loops_per_jiffy;
+			lpj->freq = freqs.old;
+		}
+		per_cpu(cpu_data, i).loops_per_jiffy = cpufreq_scale(lpj->ref, 
+														lpj->freq, freqs.new);
+	}
+	/* And don't forget to adjust the global one */
+	if (!global_lpj_ref.freq) {
+		global_lpj_ref.ref = loops_per_jiffy;
+		global_lpj_ref.freq = freqs.old;
+	}
+	loops_per_jiffy = cpufreq_scale(global_lpj_ref.ref, 
+									global_lpj_ref.freq, freqs.new);
+}
+#else
+static void wmt_update_lpj(struct cpufreq_policy *policy, struct cpufreq_freqs freqs)
+{
+	return ;
+}
+#endif
+
+/* target is calculated by cpufreq governor, unit Khz */
+static int
+wmt_target(struct cpufreq_policy *policy, unsigned target, unsigned relation)
+{
+	int ret = 0;
+	unsigned int i = 0;
+	struct cpufreq_freqs freqs;
+	struct wmt_dvfs_table *dvfs_tbl = NULL;
+
+	if (policy->cpu > NR_CPUS)
+		return -EINVAL;
+
+	fq_dbg("cpu freq:%dMhz now, need %s to %dMhz\n", wmt_getspeed(policy->cpu) / 1000,
+				(relation == CPUFREQ_RELATION_L) ? "DOWN" : "UP", target / 1000);
+	if (use_regulator) {
+		if (regulator_is_enabled(re) < 0) {
+			ret = -EBUSY;
+			fq_dbg("regulator is disabled\n\n");
+			goto out;
+		}
+	}
+
+	/* Ensure desired rate is within allowed range.  Some govenors
+	 * (ondemand) will just pass target_freq=0 to get the minimum. */
+	
+	if ((strncmp(policy->governor->name, "performance", CPUFREQ_NAME_LEN)) && (wmt_dvfs_drvdata.tbl_num > 2))
+	{		
+		if (policy->max >= wmt_dvfs_drvdata.dvfs_table[(wmt_dvfs_drvdata.tbl_num - 1)].freq)
+			policy->max = wmt_dvfs_drvdata.dvfs_table[(wmt_dvfs_drvdata.tbl_num - 2)].freq;
+		if (policy->min >= wmt_dvfs_drvdata.dvfs_table[(wmt_dvfs_drvdata.tbl_num - 1)].freq)
+			policy->min = wmt_dvfs_drvdata.dvfs_table[(wmt_dvfs_drvdata.tbl_num - 2)].freq;
+	}
+	 
+	if (target < policy->min)
+		target = policy->min;
+	if (target > policy->max)
+		target = policy->max;
+
+	/* find out (freq, voltage) pair to do dvfs */
+	dvfs_tbl = wmt_recalc_target_freq(&target, relation);
+	if (dvfs_tbl == NULL) {
+		fq_dbg("Can not change to target_freq:%dKhz", target);
+		ret = -EINVAL;
+		goto out;
+	}
+	fq_dbg("recalculated target freq is %dMhz\n", target / 1000);
+
+	freqs.cpu = policy->cpu;
+	freqs.new = target;
+	freqs.old = wmt_getspeed(policy->cpu);
+
+	if (freqs.new == freqs.old) {
+		ret = 0;
+		goto out;
+	} else if (freqs.new > freqs.old)
+		relation = CPUFREQ_RELATION_H;
+	else
+		relation = CPUFREQ_RELATION_L;
+
+	/* notifier for all cpus */
+	for_each_cpu(i, policy->cpus) {
+		freqs.cpu = i;
+		cpufreq_notify_transition(&freqs, CPUFREQ_PRECHANGE);
+	}
+	
+	/* actually we just scaling CPU frequency here */
+	get_arm_plla_param();
+	ret = wmt_change_plla_table(dvfs_tbl, relation);
+	get_arm_plla_param();
+	fq_dbg("change to %dKhz\n", ret / 1000);
+
+	if (ret < 0) {
+		ret = -EFAULT;
+		fq_dbg("wmt_cpufreq: auto_pll_divisor failed\n");
+	} else {
+		wmt_update_lpj(policy, freqs);
+
+		for_each_cpu(i, policy->cpus) {
+			freqs.cpu = i;
+			cpufreq_notify_transition(&freqs, CPUFREQ_POSTCHANGE);
+		}
+	}
+
+out:
+	fq_dbg("cpu freq scaled to %dMhz now\n\n", wmt_getspeed(policy->cpu) / 1000);
+	return ret;
+}
+
+static int
+wmt_dvfs_target(struct cpufreq_policy *policy, unsigned target, unsigned relation)
+{
+	int ret = 0;
+
+	down(&wmt_cpufreq_sem);
+
+	if (!wmt_dvfs_running) {
+		ret = -ENODEV;
+		fq_dbg("dvfs is not running now!\n");
+		goto out;
+	}
+
+	ret = wmt_target(policy, target, relation);
+	
+out:
+	up(&wmt_cpufreq_sem);
+	fq_dbg("cpu freq scaled to %dMhz now\n\n", wmt_getspeed(policy->cpu) / 1000);
+	return ret;
+}
+
+
+static int
+wmt_cpufreq_pm_notify(struct notifier_block *nb, unsigned long event, void *dummy)
+{
+	struct cpufreq_policy *policy = cpufreq_cpu_get(0);
+	int default_voltage = 0;
+	struct plla_param plla_env;
+	struct wmt_dvfs_table *plla_table = NULL;
+
+	plla_table = &wmt_dvfs_drvdata.dvfs_table[0];
+
+	plla_env.plla_clk = (plla_table->freq / 1000);
+	plla_env.arm_div = 1;
+	plla_env.l2c_div = plla_table->l2c_div;
+	plla_env.l2c_tag_div = plla_table->l2c_tag;
+	plla_env.l2c_data_div = plla_table->l2c_data;
+	plla_env.axi_div = plla_table->axi;
+	plla_env.tb_index = plla_table->index;
+
+	if (event == PM_SUSPEND_PREPARE)
+		wmt_suspend_target(0, CPUFREQ_RELATION_L, 1);
+	else if (event == PM_POST_SUSPEND) {
+		if (use_regulator) {
+			default_voltage = regulator_get_voltage(re);
+			if (use_dvfs_debug)
+				printk("default_voltage = %d\n", default_voltage);
+			regulator_set_voltage(re, default_voltage, default_voltage);
+			if (use_dvfs_debug)
+				printk("Set Min. Freq = %d\n", plla_env.plla_clk);
+			set_plla_divisor(&plla_env);
+		}
+		down(&wmt_cpufreq_sem);
+		wmt_dvfs_running = 1;
+		wmt_target(policy, freq_save, CPUFREQ_RELATION_H);
+		up(&wmt_cpufreq_sem);
+		/* when resume back, changed to suspended freq */
+	}
+
+	return NOTIFY_OK;
+}
+
+static struct notifier_block wmt_cpu_pm_notifier = {
+	.notifier_call = wmt_cpufreq_pm_notify,
+};
+
+static int wmt_verify_speed(struct cpufreq_policy *policy)
+{
+	int ret = 0;
+	struct cpufreq_frequency_table *freq_tbl = wmt_dvfs_drvdata.freq_table;
+
+	if (policy->cpu >= NR_CPUS)
+		return -EINVAL;
+
+	if (NULL == freq_tbl)
+		ret = -EINVAL;
+	else
+		ret = cpufreq_frequency_table_verify(policy, freq_tbl);
+
+	return ret;
+}
+
+static int wmt_cpu_init(struct cpufreq_policy *policy)
+{
+	int ret = 0;
+	struct cpufreq_frequency_table *wmt_freq_tbl = NULL;
+
+	fq_dbg("CPU%d cpufreq init\n", policy->cpu);
+	if (policy->cpu >= NR_CPUS)
+		return -EINVAL;
+
+	wmt_freq_tbl = wmt_dvfs_drvdata.freq_table;
+	if (!wmt_freq_tbl)
+		return -EINVAL;
+
+	policy->cur = wmt_getspeed(policy->cpu);
+	policy->min = wmt_getspeed(policy->cpu);
+	policy->max = wmt_getspeed(policy->cpu);
+
+	/*
+	 * check each frequency and find max_freq
+	 * min_freq in the table, then set:
+	 *  policy->min = policy->cpuinfo.min_freq = min_freq;
+	 *  policy->max = policy->cpuinfo.max_freq = max_freq;
+	 */
+	ret = cpufreq_frequency_table_cpuinfo(policy, wmt_freq_tbl);
+	if (0 == ret)
+		cpufreq_frequency_table_get_attr(wmt_freq_tbl, policy->cpu);
+
+	/*
+	 * On ARM-CortaxA9 configuartion, both processors share the voltage
+	 * and clock. So both CPUs needs to be scaled together and hence
+	 * needs software co-ordination. Use cpufreq affected_cpus
+	 * interface to handle this scenario. Additional is_smp() check
+	 * is to keep SMP_ON_UP build working.
+	 */
+	if (is_smp()) {
+	    policy->shared_type = CPUFREQ_SHARED_TYPE_ANY;
+	    cpumask_setall(policy->cpus);
+	}
+	/* only CPU0 can enable dvfs when cpufreq init.
+	 * when cpu hotplug enable, cpu1 will re-init when plugin */
+	if (!policy->cpu) {
+		down(&wmt_cpufreq_sem);
+		wmt_dvfs_running = 1;
+		wmt_target(policy, policy->max, CPUFREQ_RELATION_H);
+		up(&wmt_cpufreq_sem);
+	}
+	/*
+	 * 1. make sure current frequency be covered in cpufreq_table
+	 * 2. change cpu frequency to policy-max for fast booting
+	 */
+
+	policy->cur = wmt_getspeed(policy->cpu);
+	policy->cpuinfo.transition_latency = wmt_dvfs_drvdata.sample_rate;
+
+	fq_dbg("CPU%d,p->max:%d, p->min:%d, c->max=%d, c->min=%d, current:%d\n",
+			policy->cpu, policy->max, policy->min, policy->cpuinfo.max_freq,
+			policy->cpuinfo.min_freq, wmt_getspeed(policy->cpu));
+
+	return ret;
+}
+
+static struct freq_attr *wmt_cpufreq_attr[] = {
+	&cpufreq_freq_attr_scaling_available_freqs,
+	NULL,
+};
+
+static struct cpufreq_driver wmt_cpufreq_driver = {
+	.name       = "wmt_cpufreq",
+	.owner      = THIS_MODULE,
+	.flags		= CPUFREQ_STICKY,
+	.init       = wmt_cpu_init,
+	.verify     = wmt_verify_speed,
+	.target     = wmt_dvfs_target,
+	.get        = wmt_getspeed,
+	.attr       = wmt_cpufreq_attr,
+};
+
+static int __init wmt_cpufreq_check_env(void)
+{
+	int i = 0;
+	int ret = 0;
+	int varlen = 512;
+	char *ptr = NULL;
+	unsigned int tbl_num = 0;
+	unsigned int drv_en = 0;
+	unsigned int sample_rate = 0;
+	unsigned int freq = 0;
+	unsigned int voltage = 0;
+	unsigned int l2c_div = 0;
+	unsigned int l2c_tag = 0;
+	unsigned int l2c_data = 0;
+	unsigned int axi = 0;
+	struct wmt_dvfs_table *wmt_dvfs_tbl = NULL;
+	unsigned char buf[512] = {0};
+	char	f_is_bonding = 0;
+
+	/* uboot env name is: wmt.cpufreq.param, format is:
+	<enable>:<sample_rate>:<table_number>:<[freq,voltage,l2c_div,l2c_tag,l2c_data,axi]
+	*/
+
+	use_dvfs = 0;
+	use_dvfs_debug = 0;
+
+	if (BONDING_OPTION_4BYTE_VAL & 0x8)
+		f_is_bonding = 1;
+	
+	if (f_is_bonding)
+		ret = wmt_getsyspara("wmt.cpufreqlp.param", buf, &varlen);	
+	else
+		ret = wmt_getsyspara("wmt.cpufreq.param", buf, &varlen);
+		
+	if (ret) {
+		printk(KERN_INFO "Can not find uboot env wmt.cpufreq.param\n");
+		ret = -ENODATA;
+		goto out;
+	}
+	fq_dbg("wmt.cpufreq.param:%s\n", buf);
+
+	sscanf(buf, "%x:%d:%d", &drv_en, &sample_rate, &tbl_num);
+	if (!drv_en) {
+		printk(KERN_INFO "wmt cpufreq disaled\n");
+		ret = -ENODEV;
+		goto out;
+	}
+
+	/* 2 dvfs table at least */
+	if (tbl_num < DVFS_TABLE_NUM_MIN) {
+		printk(KERN_INFO "No frequency information found\n");
+		ret = -ENODATA;
+		goto out;
+	}
+	if (tbl_num > DVFS_TABLE_NUM_MAX)
+		tbl_num = DVFS_TABLE_NUM_MAX;
+
+	wmt_dvfs_tbl = kzalloc(sizeof(struct wmt_dvfs_table) * tbl_num, GFP_KERNEL);
+	if (!wmt_dvfs_tbl) {
+		ret = -ENOMEM;
+		goto out;
+	}
+	wmt_dvfs_drvdata.tbl_num = tbl_num;
+	wmt_dvfs_drvdata.sample_rate = sample_rate * MILLISEC_TO_MICROSEC;
+	wmt_dvfs_drvdata.dvfs_table = wmt_dvfs_tbl;
+	INIT_LIST_HEAD(&wmt_dvfs_drvdata.wmt_dvfs_list);
+
+	/* copy freq&vol info from uboot env to wmt_dvfs_table */
+	ptr = buf;
+	for (i = 0; i < tbl_num; i++) {
+		strsep(&ptr, "[");
+		sscanf(ptr, "%d,%d,%d,%d,%d,%d]:[", &freq, &voltage, &l2c_div, &l2c_tag, &l2c_data, &axi);
+		wmt_dvfs_tbl[i].freq = freq*1000;
+		wmt_dvfs_tbl[i].vol  = voltage;
+		wmt_dvfs_tbl[i].l2c_div = l2c_div;
+		wmt_dvfs_tbl[i].l2c_tag  = l2c_tag;
+		wmt_dvfs_tbl[i].l2c_data = l2c_data;
+		wmt_dvfs_tbl[i].axi = axi;
+		wmt_dvfs_tbl[i].index = i;
+		INIT_LIST_HEAD(&wmt_dvfs_tbl[i].node);
+		list_add_tail(&wmt_dvfs_tbl[i].node, &wmt_dvfs_drvdata.wmt_dvfs_list);
+		fq_dbg("dvfs_table[%d]: freq %dMhz, voltage %dmV l2c_div %d l2c_tag %d l2c_data %d axi %d\n",
+			i, freq, voltage, l2c_div, l2c_tag, l2c_data, axi);
+//		printk("dvfs_table[%d]: freq %dMhz, voltage %dmV l2c_div %d l2c_tag %d l2c_data %d axi %d \n",
+//			i, freq, voltage, l2c_div, l2c_tag, l2c_data, axi);//gri
+	}
+	use_dvfs = 1;
+
+	if (drv_en & 0x10)
+		use_dvfs_debug = 1;
+
+out:
+	return ret;
+}
+
+static int __init wmt_cpufreq_driver_init(void)
+{
+	int ret = 0;
+	unsigned int chip_id = 0;
+	unsigned int bondingid = 0;
+	struct cpufreq_frequency_table *wmt_freq_tbl = NULL;
+
+	wmt_dvfs_running = 0;
+	sema_init(&wmt_cpufreq_sem, 1);
+	wmt_getsocinfo(&chip_id, &bondingid);
+
+	/* if cpufreq disabled, cpu will always run at current frequency
+	 * which defined in wmt.plla.param */
+	ret = wmt_cpufreq_check_env();
+	if (ret) {
+		printk(KERN_WARNING "wmt_cpufreq check env failed, current cpu "
+				"frequency is %dKhz\n", wmt_getspeed(0));
+		goto out;
+	}
+	/* copy dvfs info from uboot to cpufreq table, 
+		generate cpu frequency table here */
+	wmt_init_cpufreq_table(&wmt_dvfs_drvdata);
+	wmt_freq_tbl = wmt_dvfs_drvdata.freq_table;
+	if (NULL == wmt_freq_tbl) {
+		printk(KERN_ERR "wmt_cpufreq create wmt_freq_tbl failed\n");
+		ret = -EINVAL;
+		goto out;
+	}
+
+	register_reboot_notifier(&wmt_reboot_notifier);
+	register_pm_notifier(&wmt_cpu_pm_notifier);
+
+	use_regulator = 1;
+	re = regulator_get(NULL, "wmt_corepower");
+	if (IS_ERR(re))
+		use_regulator = 0;
+	printk("[CPU-FREQ]use_regulator = %d\n", use_regulator);
+	ret = cpufreq_register_driver(&wmt_cpufreq_driver);
+
+out:
+	return ret;
+}
+late_initcall(wmt_cpufreq_driver_init);
+
+MODULE_AUTHOR("WonderMedia Technologies, Inc");
+MODULE_DESCRIPTION("WMT CPU frequency driver");
+MODULE_LICENSE("Dual BSD/GPL");
diff --git a/arch/arm/mach-wmt/wmt_time.c b/arch/arm/mach-wmt/wmt_time.c
new file mode 100644
index 00000000..e4f6093a
--- /dev/null
+++ b/arch/arm/mach-wmt/wmt_time.c
@@ -0,0 +1,460 @@
+/*++
+linux/arch/arm/mach-wmt/wmt_time.c
+
+Copyright (c) 2008  WonderMedia Technologies, Inc.
+
+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, see <http://www.gnu.org/licenses/>.
+
+WonderMedia Technologies, Inc.
+10F, 529, Chung-Cheng Road, Hsin-Tien, Taipei 231, R.O.C.
+--*/
+
+#include <linux/io.h>
+#include <linux/irq.h>
+#include <linux/interrupt.h>
+#include <linux/clocksource.h>
+#include <linux/clockchips.h>
+#include <linux/delay.h>
+#include <linux/export.h>
+
+#include <asm/mach/time.h>
+#include <asm/sched_clock.h>
+#include <mach/hardware.h>
+#include <mach/wmt_mmap.h>
+#include <mach/wmt_env.h>
+#include <asm/smp_twd.h>
+
+//#define DEBUG
+#ifdef  DEBUG
+#define fq_dbg(fmt, args...)  printk(KERN_ERR "[%s]_%d: " fmt, __func__ , __LINE__, ## args)
+#define fq_trace()  printk(KERN_ERR "trace in %s %d\n", __func__, __LINE__)
+#else
+#define fq_dbg(fmt, args...)
+#define fq_trace()
+#endif
+
+#define MIN_OSCR_DELTA  16
+#define MIN_HRTMR_CYC_DELTA  64
+#define WMT_CLOCK_TICK_RATE 3000000
+#define WMT_CLOCK_TICK_RATE1 5000000
+#define WMT_CLOCK_TICK_RATE2 6000000
+#define WMT_CLOCK_TICK_RATE3 6000000
+
+/* Clear OS Timer1 irq */
+static inline void wmt_os_timer_clear_irq(void)
+{
+	OSTS_VAL = OSTS_M1;
+}
+
+/* disable OS Timer1 irq */
+static inline void wmt_os_timer_disable_irq(void)
+{
+	OSTI_VAL &= ~OSTI_E1;
+}
+
+/* Enable OS timer1 irq */
+static inline void wmt_os_timer_enable_irq(void)
+{
+	OSTI_VAL |= OSTI_E1;
+}
+
+/* Stop ostimer, counter stop */
+static inline void wmt_os_timer_freeze_counter(void)
+{
+	OSTC_VAL = 0;
+}
+
+/* Let OS Timer free run, counter increase now */
+static inline void wmt_os_timer_restart_counter(void)
+{
+	OSTC_VAL = OSTC_ENABLE;
+}
+
+static inline void wmt_os_timer_set_counter(u32 new_cnt)
+{
+	OSCR_VAL = new_cnt;
+}
+
+static inline u32 wmt_os_timer_read_counter(void)
+{
+	OSTC_VAL |= OSTC_RDREQ;
+	while (OSTA_VAL & OSTA_RCA)
+		;
+
+	return (u32)OSCR_VAL;
+}
+
+static inline void wmt_os_timer_set_match(u32 new_match)
+{
+	/* check if can write OS Timer1 match register nows */
+	while (OSTA_VAL & OSTA_MWA1)
+		;
+
+	OSM1_VAL = new_match;
+}
+
+/* OS timer hardware initializing routine */
+/* TODO: Here we let os timer run, but disable interrupt, 
+         when clcokevent registed, then enable interrupt
+*/
+static void __init wmt_os_timer_init(void)
+{
+	wmt_os_timer_disable_irq();
+	wmt_os_timer_clear_irq();
+	wmt_os_timer_freeze_counter();
+	wmt_os_timer_set_match(0);
+	wmt_os_timer_restart_counter();
+}
+
+/* for clocksource */
+static cycle_t wmt_timer_read_cycles(struct clocksource *cs)
+{
+	return (cycle_t)wmt_os_timer_read_counter();
+}
+
+struct clocksource wmt_clocksource = {
+	.name           = "wmt_clocksource",
+	.rating         = 200,
+	.read           = wmt_timer_read_cycles,
+	.mask           = CLOCKSOURCE_MASK(32),
+	.flags          = CLOCK_SOURCE_IS_CONTINUOUS,
+};
+
+static void __init wmt_clocksource_init(struct clocksource *cs)
+{
+	clocksource_register_hz(cs, WMT_CLOCK_TICK_RATE);
+	fq_dbg("%s mult:%u, shift:%u, max_idle_ns:%llu\n\n", cs->name,
+		cs->mult, cs->shift, cs->max_idle_ns);
+}
+
+struct clocksource wmt_clocksource1 = {
+	.name           = "wmt_clocksource1",
+	.rating         = 150,
+	.read           = wmt_timer_read_cycles,
+	.mask           = CLOCKSOURCE_MASK(32),
+	.flags          = CLOCK_SOURCE_IS_CONTINUOUS,
+};
+
+static void __init wmt_clocksource_init1(struct clocksource *cs)
+{
+	clocksource_register_hz(cs, WMT_CLOCK_TICK_RATE1);
+    fq_dbg("%s mult:%u, shift:%u, max_idle_ns:%llu\n\n", cs->name,
+                                cs->mult, cs->shift, cs->max_idle_ns);
+}
+
+
+struct clocksource wmt_clocksource2 = {
+	.name           = "wmt_clocksource2",
+	.rating         = 150,
+	.read           = wmt_timer_read_cycles,
+	.mask           = CLOCKSOURCE_MASK(32),
+	.flags          = CLOCK_SOURCE_IS_CONTINUOUS,
+};
+
+static void __init wmt_clocksource_init2(struct clocksource *cs)
+{
+	clocksource_register_hz(cs, WMT_CLOCK_TICK_RATE2);
+    fq_dbg("%s mult:%u, shift:%u, max_idle_ns:%llu\n\n", cs->name,
+                                cs->mult, cs->shift, cs->max_idle_ns);
+}
+
+struct clocksource wmt_clocksource3 = {
+	.name           = "wmt_clocksource3",
+	.rating         = 150,
+	.read           = wmt_timer_read_cycles,
+	.mask           = CLOCKSOURCE_MASK(32),
+	.flags          = CLOCK_SOURCE_IS_CONTINUOUS,
+};
+
+static void __init wmt_clocksource_init3(struct clocksource *cs)
+{
+	clocksource_register_hz(cs, WMT_CLOCK_TICK_RATE3);
+    fq_dbg("%s mult:%u, shift:%u, max_idle_ns:%llu\n\n", cs->name,
+                                cs->mult, cs->shift, cs->max_idle_ns);
+}
+
+
+static int
+wmt_timer_set_next_event(unsigned long cycles, struct clock_event_device *evt)
+{
+	unsigned long next = 0;
+	unsigned long oscr = 0;
+
+	oscr = wmt_os_timer_read_counter();
+	next = oscr + cycles;
+	/* set new value to os time1 match register   */
+	wmt_os_timer_set_match(next);
+	/* Enable match on timer 1 to cause interrupts. */
+	wmt_os_timer_enable_irq();
+	/* check if overflow */
+	if ((signed long)(next - wmt_os_timer_read_counter()) <= MIN_OSCR_DELTA) {
+		fq_dbg("set os timer overflow!, set_cyc:%lu, now_cyc:%lu,"
+				" next_cyc:%lu\n\n\n", cycles, oscr, next);
+		return -ETIME;
+	}
+
+	return 0;
+}
+
+static void
+wmt_timer_set_mode(enum clock_event_mode mode, struct clock_event_device *evt)
+{
+	switch (mode) {
+	case CLOCK_EVT_MODE_UNUSED:
+	case CLOCK_EVT_MODE_SHUTDOWN:
+	case CLOCK_EVT_MODE_ONESHOT:
+		/* disable OS Timer irq here */
+		wmt_os_timer_disable_irq();
+		/* Clear match on OS Timer 1 */
+		wmt_os_timer_clear_irq();
+		break;
+	case CLOCK_EVT_MODE_RESUME:
+	case CLOCK_EVT_MODE_PERIODIC:
+	default:
+		break;
+	}
+
+	return ;
+}
+
+struct clock_event_device wmt_clockevent = {
+	.name           = "wmt_clockevent",
+	.features       = CLOCK_EVT_FEAT_ONESHOT,
+	.rating         = 200,
+	.set_next_event = wmt_timer_set_next_event,
+	.set_mode       = wmt_timer_set_mode,
+	.shift          = 32,
+};
+
+static irqreturn_t wmt_timer_interrupt(int irq, void *dev_id)
+{
+	struct clock_event_device *evt = dev_id;
+
+	/* Clear match on OS Timer 1 irq */
+	wmt_os_timer_clear_irq();
+	evt->event_handler(evt);
+
+	return IRQ_HANDLED;
+}
+
+struct irqaction wmt_timer_irq = {
+	.name    = "wmt_timer",
+	.flags   = IRQF_DISABLED | IRQF_TIMER | IRQF_IRQPOLL,
+	.handler = wmt_timer_interrupt,
+	.dev_id  = &wmt_clockevent,
+};
+
+static void __init wmt_clockevent_init(struct clock_event_device *evt,
+				unsigned int irq, struct irqaction *act)
+{
+	/* reset some elements for clockevent */
+	evt->cpumask = cpumask_of(smp_processor_id());
+	evt->mult = div_sc(WMT_CLOCK_TICK_RATE, NSEC_PER_SEC, evt->shift);
+	evt->max_delta_ns = clockevent_delta2ns(0x7fffffff, evt);
+	evt->min_delta_ns = clockevent_delta2ns(MIN_HRTMR_CYC_DELTA, evt);
+	fq_dbg("%s, mult:%u, shift:%d, max_delta:%llu, min_delta:%llu\n\n\n",
+	evt->name, evt->mult, evt->shift, evt->max_delta_ns, evt->min_delta_ns);
+
+	/* setup os timer1 irq */
+	if (setup_irq(irq, act))
+		printk(KERN_ERR "setup clockevent %s irq %u, failed!\n\n", evt->name, irq);
+
+	/* register clockevent */
+	clockevents_register_device(evt);
+
+	return ;
+}
+
+/* for sched_clock */
+static u32 notrace wmt_read_sched_clock(void)
+{
+	return wmt_os_timer_read_counter();
+}
+
+/* for MPcore local timer */
+#ifdef CONFIG_LOCAL_TIMERS
+extern void wmt3498_init_clocks(void);
+#define WMT_LOCAL_TIMER_BASE (0xD8018000 + 0x600)
+#define WMT_LOCAL_TIMER_PPI_NUM 29
+static DEFINE_TWD_LOCAL_TIMER(wmt_local_timer,
+					WMT_LOCAL_TIMER_BASE, WMT_LOCAL_TIMER_PPI_NUM);
+
+/* check if uboot env wmt.twd.disable = 1, if twd disable (=1), return 1, 
+ *	if twd enable, return 0
+ */
+static int wmt_twd_is_disabled(void)
+{
+    int ret = 0;
+    int varlen = 128;
+    unsigned int drv_en = 0;
+    unsigned char buf[128] = {0};
+
+    /* uboot env name is: wmt.twd.disable */
+    ret = wmt_getsyspara("wmt.twd.disable", buf, &varlen);                                                                    
+    if (ret) {
+        ret = 0;
+        goto out;
+    }
+
+    sscanf(buf, "%d", &drv_en);
+    if (drv_en) {
+        ret = -ENODEV;
+        goto out;
+    }
+
+out:
+    return ret;
+}
+#endif
+
+static void __init wmt_timer_init(void)
+{
+	/* prepare OS timer hardware, irq disabled  */
+	wmt_os_timer_init();
+	/* os timer1 as clocksourece                */
+	wmt_clocksource_init(&wmt_clocksource);
+    	wmt_clocksource_init1(&wmt_clocksource1);	
+    	wmt_clocksource_init2(&wmt_clocksource2);	
+    	wmt_clocksource_init3(&wmt_clocksource3);	
+	/* sched_clock for timestamp, as printk.... */
+	setup_sched_clock(wmt_read_sched_clock, 32, WMT_CLOCK_TICK_RATE);
+	/* os timer1 as clockevent device           */
+	wmt_clockevent_init(&wmt_clockevent, IRQ_OST1, &wmt_timer_irq);
+
+#ifdef CONFIG_LOCAL_TIMERS
+	wmt3498_init_clocks();
+	if (wmt_twd_is_disabled()) {
+		printk(KERN_INFO "WMT local timer disabled\n");
+		goto out;
+	}
+	if (twd_local_timer_register(&wmt_local_timer))
+		printk(KERN_ERR "Register ARM local timer failed! Use broadcast mode!\n");
+#endif
+out:
+	/* this is a MUST operation */    
+	wmt_os_timer_enable_irq();
+
+	return ;
+}
+
+struct sys_timer wmt_timer = {
+	.init = wmt_timer_init
+};
+
+
+/* below code is for old design compatibility */
+inline unsigned int wmt_read_oscr(void)
+{
+	return wmt_os_timer_read_counter();
+}
+EXPORT_SYMBOL(wmt_read_oscr);
+
+int wmt_rtc_on = 1;
+static int __init no_rtc(char *str)
+{
+	wmt_rtc_on = 0;
+	return 1;
+}
+__setup("nortc", no_rtc);
+
+
+static void wmt_rtc_init(void)
+{
+	fq_dbg("Enter\n");
+
+	RTCC_VAL = (RTCC_ENA|RTCC_INTTYPE);
+	if (!(RTSR_VAL&RTSR_VAILD))
+		while (!(RTSR_VAL&RTSR_VAILD))
+			;
+	/* Reset RTC alarm settings */
+	//RTAS_VAL = 0;
+	/* Reset RTC test mode. */
+	RTTM_VAL = 0;
+
+	/* Patch 1, RTCD default value isn't 0x41 and it will not sync with RTDS. */
+	if (RTCD_VAL == 0) {
+		while (RTWS_VAL & RTWS_DATESET)
+			;
+		RTDS_VAL = RTDS_VAL;
+	}
+
+	/*
+	 * Disable all RTC control functions.
+	 * Set to 24-hr format and update type to each second.
+	 * Disable sec/min update interrupt.
+	 * Let RTC free run without interrupts.
+	 */
+	/* RTCC_VAL &= ~(RTCC_12HR | RTCC_INTENA); */
+	/* RTCC_VAL |= RTCC_ENA | RTCC_INTTYPE; */
+	RTCC_VAL = (RTCC_ENA|RTCC_INTTYPE);
+
+	if (RTWS_VAL & RTWS_CONTROL) {
+		while (RTWS_VAL & RTWS_CONTROL)
+			;
+	}
+
+	fq_dbg("Exit\n\n\n");
+	return ;
+}
+
+void wmt_read_rtc(unsigned int *date, unsigned int *time)
+{
+	/* before read rtc, we should check if rtc already be initialized */
+	if (((RTCC_VAL & RTCC_ENA) == 0) || ((RTSR_VAL & RTSR_VAILD) == 0)) {
+		wmt_rtc_init();
+	}
+
+	if (!(RTSR_VAL & RTSR_VAILD))
+		while (!(RTSR_VAL & RTSR_VAILD))
+			;
+
+	if (RTWS_VAL & RTWS_DATESET)
+		while (RTWS_VAL & RTWS_DATESET)
+			;
+
+	*date = RTCD_VAL;
+
+	if (RTWS_VAL & RTWS_TIMESET)
+		while (RTWS_VAL & RTWS_TIMESET)
+			;
+
+	*time = RTCT_VAL;
+}
+EXPORT_SYMBOL(wmt_read_rtc);
+
+/*
+ * get current Gregorian date from RTC, 
+ * coverts to seconds since 1970-01-01 00:00:00
+ */
+static unsigned long wmt_get_rtc_time(void)
+{
+	unsigned int date = 0;
+	unsigned int time = 0;
+
+	/* if no rtc or rtc disabled, return 0,
+	 * means timekeeping is 1970-01-01 00:00:00 when system booting */
+	if (!wmt_rtc_on)
+		return 0;
+
+	wmt_read_rtc(&date, &time);
+	return mktime(RTCD_CENT(date)? 2100+RTCD_YEAR(date) : 2000+RTCD_YEAR(date),
+				  RTCD_MON(date), RTCD_MDAY(date),RTCT_HOUR(time),
+				  RTCT_MIN(time), RTCT_SEC(time));
+}
+
+void read_persistent_clock(struct timespec *ts)
+{
+	ts->tv_nsec = 0;
+	ts->tv_sec  = (__kernel_time_t)wmt_get_rtc_time();
+
+	fq_dbg("seconds read from RTC is %lu\n\n\n", ts->tv_sec);
+	return ;  
+}
diff --git a/drivers/media/video/wmt/com-lock.h b/drivers/media/video/wmt/com-lock.h
new file mode 100644
index 00000000..477ddca2
--- /dev/null
+++ b/drivers/media/video/wmt/com-lock.h
@@ -0,0 +1,58 @@
+/*++
+ * WonderMedia Codec Lock driver
+ *
+ * Copyright (c) 2013  WonderMedia  Technologies, Inc.
+ *
+ * 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, see <http://www.gnu.org/licenses/>.
+ *
+ * WonderMedia Technologies, Inc.
+ * 4F, 533, Chung-Cheng Road, Hsin-Tien, Taipei 231, R.O.C
+--*/
+#ifndef COM_LOCK_H
+/* To assert that only one occurrence is included */
+#define COM_LOCK_H
+
+/*-------------------- MODULE DEPENDENCY ---------------------------------*/
+
+/*-------------------- EXPORTED PRIVATE TYPES-----------------------------*/
+
+
+/*Define wmt lock types*/
+enum {
+	lock_jpeg = 0,  /* Lock for JPEG decoder */
+	lock_video,     /* Lock for MSVD decoders */
+	lock_max
+}; /* wmt_lock_type */
+
+struct wmt_lock {
+	long lock_type;    /* TYPE_JPEG or TYPE_VIDEO */
+	long arg2;         /* for IO_WMT_UNLOCK, the timeout value */
+	int  is_wait;      /* is someone wait for this lock? */
+};
+#define wmt_lock_ioctl_arg  struct wmt_lock
+
+/*--------------------- EXPORTED PRIVATE FUNCTIONS  ----------------------*/
+
+/*--------------------------------------------------------------------------
+	Macros below are used for driver in IOCTL
+--------------------------------------------------------------------------*/
+#define LOCK_IOC_MAGIC      'L'
+#define IO_WMT_LOCK         _IOWR(LOCK_IOC_MAGIC, 0, unsigned long)
+#define IO_WMT_UNLOCK       _IOWR(LOCK_IOC_MAGIC, 1, unsigned long)
+#define IO_WMT_CHECK_WAIT   _IOR(LOCK_IOC_MAGIC, 2, unsigned long)
+
+
+#endif /* ifndef COM_LOCK_H */
+
+/*=== END com-lock.h =====================================================*/
diff --git a/drivers/media/video/wmt/com-vd.h b/drivers/media/video/wmt/com-vd.h
new file mode 100644
index 00000000..9ad58b17
--- /dev/null
+++ b/drivers/media/video/wmt/com-vd.h
@@ -0,0 +1,73 @@
+/*++
+ * Copyright (c) 2013 WonderMedia Technologies, Inc.
+ *
+ * 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, see <http://www.gnu.org/licenses/>.
+ *
+ * WonderMedia Technologies, Inc.
+ * 4F, 533, Chung-Cheng Road, Hsin-Tien, Taipei 231, R.O.C
+--*/
+
+#ifndef COM_VD_H
+/* To assert that only one occurrence is included */
+#define COM_VD_H
+
+/*-------------------- MODULE DEPENDENCY ---------------------------------*/
+#ifdef __KERNEL__
+#include <mach/com-video.h>
+#else
+#include "com-video.h"
+#endif
+
+/*-------------------- EXPORTED PRIVATE CONSTANTS ------------------------*/
+
+#define VD_JPEG    0x0001
+#define VD_MAX     0x0002
+
+/*--------------------------------------------------------------------------
+Following macros define hardware decoder type for vd_ioctl_cmd
+--------------------------------------------------------------------------*/
+#define IOCTL_CMD_INIT(cmd, _struct_type_, vd_type, version)
+
+#define VD_IOCTL_CMD_M \
+	unsigned int    identity; /* decoder type */\
+	unsigned int    size      /* size of full structure */
+/* End of VD_IOCTL_CMD_M */
+
+/*-------------------------------------------------------------------------
+	Definitions of Struct
+--------------------------------------------------------------------------*/
+struct vd_ioctl_cmd_s {
+	VD_IOCTL_CMD_M;
+};
+#define vd_ioctl_cmd    struct vd_ioctl_cmd_s
+
+struct vd_handle_s {
+	int   vd_fd;
+	int   mb_fd;
+};
+#define vd_handle_t     struct vd_handle_s
+
+/*--------------------------------------------------------------------------
+   Macros below are used for driver in IOCTL
+--------------------------------------------------------------------------*/
+#define VD_IOC_MAGIC              'k'
+#define VD_IOC_MAXNR              1
+
+/* VDIOSET_DECODE_LOOP: application send decode data to driver
+						(blocking mode in driver) */
+#define VDIOSET_DECODE_LOOP     _IOWR(VD_IOC_MAGIC, 0, vd_ioctl_cmd)
+
+#endif /* ifndef COM_VD_H */
+
+/*=== END com-vd.h ======================================================*/
diff --git a/drivers/media/video/wmt/wmt-codec.c b/drivers/media/video/wmt/wmt-codec.c
new file mode 100644
index 00000000..393281b1
--- /dev/null
+++ b/drivers/media/video/wmt/wmt-codec.c
@@ -0,0 +1,857 @@
+/*++
+ * Common interface for WonderMedia SoC hardware encoder and decoder drivers
+ *
+ * Copyright (c) 2008-2013  WonderMedia  Technologies, Inc.
+ *
+ * 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, see <http://www.gnu.org/licenses/>.
+ *
+ * WonderMedia Technologies, Inc.
+ * 4F, 533, Chung-Cheng Road, Hsin-Tien, Taipei 231, R.O.C
+--*/
+#ifndef CODEC_C
+#define CODEC_C
+
+#include <linux/device.h>       /* For EXPORT_SYMBOL() */
+#include <linux/spinlock.h>
+#include <mach/hardware.h>
+#include <linux/module.h>
+#include <linux/slab.h>         /* for kmalloc and kfree */
+#include <linux/semaphore.h>    /* for semaphore */
+
+#include "wmt-codec.h"
+
+#define CFG_CODEC_PERFORM_EN    /* Flag for codec performance analysis */
+
+
+/*#define CODEC_DEBUG*/
+#undef CODEC_DEBUG
+#ifdef CODEC_DEBUG
+#define DBG_MSG(fmt, args...) \
+	do {\
+		printk(KERN_INFO "{%s} " fmt, __func__, ## args);\
+	} while (0)
+#else
+#define DBG_MSG(fmt, args...)
+#endif
+
+#undef DBG_ERR
+#define DBG_ERR(fmt, args...) \
+	do {\
+		printk(KERN_ERR "*E* {%s} " fmt, __func__, ## args);\
+	} while (0)
+
+#undef PRINTK
+#define PRINTK(fmt, args...)  printk(KERN_INFO "{%s} " fmt, __func__, ## args)
+
+/*#define CODEC_REG_TRACE*/
+#ifdef CODEC_REG_TRACE
+#define REG_WRITE(addr, val)      \
+	do {\
+		PRINTK("REG_SET:0x%x -> 0x%0x\n", addr, val);\
+		REG32_VAL(addr) = (val);\
+	} while (0)
+#else
+#define REG_WRITE(addr, val)      (REG32_VAL(addr) = (val))
+#endif
+
+
+struct codec_lock {
+	struct semaphore  sem;
+	unsigned int      is_init;
+	unsigned int      is_locked;
+};
+
+static struct codec_lock codec_lock[CODEC_MAX];
+static int msvd_clk_enable_cnt;
+DEFINE_MUTEX(wmt_clk_mutex);
+
+
+#ifdef CFG_CODEC_PERFORM_EN
+static int wmt_codec_debug;
+
+struct wmt_tm {
+	unsigned long  seq_id;          /* seqence number */
+	char           name[16];        /* name */
+	unsigned int   initial;         /* initial flag */
+	unsigned int   total_tm;        /* total time */
+	unsigned int   interval_tm;     /* interval time */
+	unsigned int   reset;           /* reset counter */
+	unsigned int   total_cnt;       /* total counter */
+	unsigned int   count;           /* interval counter */
+	unsigned int   max;             /* max time */
+	unsigned int   min;             /* min time */
+	unsigned int   threshold;
+	struct timeval start;           /* start time */
+	struct timeval end;             /* end time */
+} ;
+#endif
+
+/*!*************************************************************************
+* wmt_power_en
+*
+* Private Function
+*
+* \retval  0 if success
+*/
+static int wmt_power_en(int enable)
+{
+	static DEFINE_SPINLOCK(wmt_power_lock);
+	unsigned long flags = 0;
+	unsigned int  value;
+	int 	ret = 0;
+	static int wmt_open_cnt=0;
+	
+	if(enable){
+	    spin_lock_irqsave(&wmt_power_lock, flags);
+		wmt_open_cnt++;
+	    spin_unlock_irqrestore(&wmt_power_lock, flags);
+		if(wmt_open_cnt == 1){
+			value = REG32_VAL(PM_CTRL_BASE_ADDR + 0x0604);
+			*(volatile unsigned int *)(PM_CTRL_BASE_ADDR + 0x0604) = (value | 0x1);
+			while((REG32_VAL(PM_CTRL_BASE_ADDR + 0x0604) & 0xf5) != 0xf1){};
+		}
+	}else {
+	    spin_lock_irqsave(&wmt_power_lock, flags);
+	    if( wmt_open_cnt >= 1) {
+	    	wmt_open_cnt--;
+	    }else {
+	    	DBG_ERR("Unexpected WMT Codec power off, ignore it!\n");
+	    	ret = -1;
+	    }
+	    spin_unlock_irqrestore(&wmt_power_lock, flags);
+		if(wmt_open_cnt == 0){
+			value = REG32_VAL(PM_CTRL_BASE_ADDR + 0x0604);
+			*(volatile unsigned char *)(PM_CTRL_BASE_ADDR + 0x0604) = (value & ~(0x1));
+			while((REG32_VAL(PM_CTRL_BASE_ADDR + 0x0604) & 0xf5) != 0x00){};
+		}
+	}
+	return ret;
+} /* End of wmt_power_en() */
+
+/*!*************************************************************************
+* msvd_power_en
+*
+* Private Function
+*
+* \retval  0 if success
+*/
+static int msvd_power_en(int enable)
+{
+	static DEFINE_SPINLOCK(msvd_power_lock);
+	unsigned long flags = 0;
+	unsigned int  value;
+	int 	ret = 0;
+	static int msvd_open_cnt = 0;
+
+	if (enable) {
+	    spin_lock_irqsave(&msvd_power_lock, flags);
+		msvd_open_cnt++;
+	    spin_unlock_irqrestore(&msvd_power_lock, flags);
+		if(msvd_open_cnt == 1){
+			value = REG32_VAL(PM_CTRL_BASE_ADDR + 0x062C);
+			*(volatile unsigned int *)(PM_CTRL_BASE_ADDR + 0x062C) = (value | 0x1);
+			while((REG32_VAL(PM_CTRL_BASE_ADDR + 0x062C) & 0xf5) != 0xf1){};
+		}
+	} else {
+	    spin_lock_irqsave(&msvd_power_lock, flags);
+	    if( msvd_open_cnt >= 1) {
+	    	msvd_open_cnt--;
+	    }else {
+	    	DBG_ERR("Unexpected MSVD Codec power off, ignore it!\n");
+	    	ret = -1;
+	    }	    	
+	    spin_unlock_irqrestore(&msvd_power_lock, flags);
+		if(msvd_open_cnt == 0){
+			value = REG32_VAL(PM_CTRL_BASE_ADDR + 0x062C);
+			*(volatile unsigned int *)(PM_CTRL_BASE_ADDR + 0x062C) = (value & ~(0x1));
+			while((REG32_VAL(PM_CTRL_BASE_ADDR + 0x062C) & 0xf5) != 0x00){};
+		}
+	}
+	return 0;
+} /* End of msvd_power_en() */
+
+/*!*************************************************************************
+* wmt_clock_en
+* 
+* Public Function
+*/
+/*!
+* \brief
+*	Set source PRD table
+*
+* \retval  0 if success
+*/ 
+int wmt_clock_en(int codec_type, int enable)
+{
+	static DEFINE_SPINLOCK(clk_lock);
+	unsigned long flags = 0;
+
+	if ((codec_type < 0 ) || (codec_type >= CODEC_MAX)) {
+		DBG_ERR("Unsupported codec ID %d\n", codec_type);
+		return -1;
+	}
+
+	if (codec_type == CODEC_VD_JPEG) {
+		spin_lock_irqsave(&clk_lock, flags);
+		if (enable) {
+			auto_pll_divisor(DEV_JDEC, CLK_ENABLE, 0, 0);
+			auto_pll_divisor(DEV_MSVD, CLK_ENABLE, 0, 0);
+			auto_pll_divisor(DEV_WMTVDU, CLK_ENABLE, 0, 0);
+			auto_pll_divisor(DEV_WMTNA, CLK_ENABLE, 0, 0);
+		} else {
+			auto_pll_divisor(DEV_WMTNA,  CLK_DISABLE, 0, 0);
+			auto_pll_divisor(DEV_WMTVDU,  CLK_DISABLE, 0, 0);
+			auto_pll_divisor(DEV_MSVD, CLK_DISABLE, 0, 0);
+			auto_pll_divisor(DEV_JDEC, CLK_DISABLE, 0, 0);
+		}
+		spin_unlock_irqrestore(&clk_lock, flags);
+	} else if (codec_type == CODEC_VD_MSVD) {
+		mutex_lock(&wmt_clk_mutex);
+		if (enable) {
+			auto_pll_divisor(DEV_WMTNA, CLK_ENABLE, 0, 0);
+			auto_pll_divisor(DEV_CNMNA, CLK_ENABLE, 0, 0);
+			auto_pll_divisor(DEV_MSVD, CLK_ENABLE, 0, 0);
+		} else {
+			auto_pll_divisor(DEV_MSVD,  CLK_DISABLE, 0, 0);
+			auto_pll_divisor(DEV_CNMNA, CLK_DISABLE, 0, 0);
+			auto_pll_divisor(DEV_WMTNA, CLK_DISABLE, 0, 0);
+		}
+		mutex_unlock(&wmt_clk_mutex);
+	} else if (codec_type == CODEC_VE_H264) {
+		spin_lock_irqsave(&clk_lock, flags);
+		if (enable) {
+			auto_pll_divisor(DEV_WMTNA, CLK_ENABLE, 0, 0);
+			auto_pll_divisor(DEV_MSVD, CLK_ENABLE, 0, 0);
+			auto_pll_divisor(DEV_WMTVDU, CLK_ENABLE, 0, 0);
+			auto_pll_divisor(DEV_H264, CLK_ENABLE, 0, 0);
+		} else {
+			auto_pll_divisor(DEV_H264, CLK_DISABLE, 0, 0);
+			auto_pll_divisor(DEV_MSVD, CLK_DISABLE, 0, 0);
+			auto_pll_divisor(DEV_WMTNA, CLK_DISABLE, 0, 0);
+			auto_pll_divisor(DEV_WMTVDU, CLK_DISABLE, 0, 0);
+		}
+		spin_unlock_irqrestore(&clk_lock, flags);
+	} else if (codec_type == CODEC_VE_JPEG) {
+		spin_lock_irqsave(&clk_lock, flags);
+		if (enable) {
+			auto_pll_divisor(DEV_JENC, CLK_ENABLE, 0, 0);
+			auto_pll_divisor(DEV_MSVD, CLK_ENABLE, 0, 0);
+			auto_pll_divisor(DEV_WMTVDU,  CLK_ENABLE, 0, 0);
+			auto_pll_divisor(DEV_WMTNA,  CLK_ENABLE, 0, 0);
+		} else {
+			auto_pll_divisor(DEV_WMTNA,  CLK_DISABLE, 0, 0);
+			auto_pll_divisor(DEV_WMTVDU,  CLK_DISABLE, 0, 0);
+			auto_pll_divisor(DEV_MSVD, CLK_DISABLE, 0, 0);
+			auto_pll_divisor(DEV_JENC, CLK_DISABLE, 0, 0);
+		}
+		spin_unlock_irqrestore(&clk_lock, flags);
+	}
+	return 0;
+} /* End of wmt_clock_en() */
+
+/*!*************************************************************************
+* wmt_get_codec_clock_count
+*
+* Private Function
+*
+* \retval  0 if success
+*/
+int wmt_get_codec_clock_count(void)
+{
+	return msvd_clk_enable_cnt;
+}
+EXPORT_SYMBOL(wmt_get_codec_clock_count);
+
+/*!*************************************************************************
+* wmt_reset_codec_clock_count
+*
+* Private Function
+*
+* \retval  0 if success
+*/
+void wmt_reset_codec_clock_count(void)
+{
+	msvd_clk_enable_cnt = 0;
+}
+EXPORT_SYMBOL(wmt_reset_codec_clock_count);
+
+/*!*************************************************************************
+* wmt_codec_clock_en
+*
+* Public Function
+*/
+/*!
+* \brief
+*
+* \retval  0 if success
+*/
+int wmt_codec_clock_en(int codec_type, int enable)
+{
+    static DEFINE_SPINLOCK(clk_lock);
+    unsigned long flags = 0;
+    int ret = 0;
+	 
+    if ((codec_type < 0 ) || (codec_type >= CODEC_MAX)) {
+        DBG_ERR("Unsupported codec ID %d\n", codec_type);
+        return -1;
+    }     
+    if (codec_type == CODEC_VD_JPEG) {
+        spin_lock_irqsave(&clk_lock, flags);
+        if (enable) {
+            REG_WRITE(MSVD_BASE_ADDR + 0x020, 0x0F);
+        } else {
+            REG_WRITE(MSVD_BASE_ADDR + 0x020, 0x00);
+        }
+        spin_unlock_irqrestore(&clk_lock, flags);
+    } else if (codec_type == CODEC_VD_MSVD) {
+        mutex_lock(&wmt_clk_mutex);
+        if(enable) {
+            msvd_clk_enable_cnt++; 	
+            if(msvd_clk_enable_cnt == 1){
+                auto_pll_divisor(DEV_CNMVDU, CLK_ENABLE, 0, 0);
+                REG_WRITE(MSVD_BASE_ADDR + 0x000, 0x07);
+            }
+        }else {
+            if( msvd_clk_enable_cnt >= 1) {
+                msvd_clk_enable_cnt--;
+                if(msvd_clk_enable_cnt == 0){
+                    REG_WRITE(MSVD_BASE_ADDR + 0x000, 0x00);
+                    auto_pll_divisor(DEV_CNMVDU, CLK_DISABLE, 0, 0);
+                }
+            }else {
+                    DBG_ERR("Unexpected MSVD Codec power off, ignore it!\n");
+                    ret = -1;
+            }	    	
+        }        
+        mutex_unlock(&wmt_clk_mutex);
+    } else if (codec_type == CODEC_VE_H264 ) {
+        spin_lock_irqsave(&clk_lock, flags);
+        if(enable) {
+            REG_WRITE(MSVD_BASE_ADDR + 0x040, 0x0F);
+        }else {
+            REG_WRITE(MSVD_BASE_ADDR + 0x040, 0x00);
+        }
+        spin_unlock_irqrestore(&clk_lock, flags);
+    } else if (codec_type == CODEC_VE_JPEG) {
+        spin_lock_irqsave(&clk_lock, flags);
+        if (enable) {
+            REG_WRITE(MSVD_BASE_ADDR + 0x060, 0x0F);
+        } else {
+            REG_WRITE(MSVD_BASE_ADDR + 0x060, 0x00);                
+        }
+        spin_unlock_irqrestore(&clk_lock, flags);
+    }
+    return ret;
+} /* End of wmt_codec_clock_en() */
+EXPORT_SYMBOL(wmt_codec_clock_en);
+
+/*!*************************************************************************
+* wmt_codec_pmc_ctl
+*
+* Public Function
+*/
+/*!
+* \brief
+*
+* \retval  0 if success
+*/
+int wmt_codec_pmc_ctl(int codec_type, int enable)
+{   
+
+    if ((codec_type < 0 ) || (codec_type >= CODEC_MAX)) {
+        DBG_ERR("Unsupported codec ID %d\n", codec_type);
+        return -1;
+    }
+    if (enable){
+    		wmt_clock_en(codec_type, 1); /* WMT  clock on */
+    		wmt_power_en(1); /* WMT Codec power cotrol  *//*PMC.0x604 */
+    		if (codec_type == CODEC_VD_MSVD)
+ 			msvd_power_en(1); /* MSVD Codec power control */ /*PMC.0x62c */
+    }else{
+		if (codec_type == CODEC_VD_MSVD)
+ 			msvd_power_en(0); /* MSVD Codec power control */ /*PMC.0x62c */
+    		wmt_power_en(0); /* WMT Codec power cotrol  *//*PMC.0x604 */
+    		
+    		wmt_clock_en(codec_type, 0); /* WMT  clock on */
+    }
+	return 0;
+} /* End of wmt_codec_pmc_ctl() */
+EXPORT_SYMBOL(wmt_codec_pmc_ctl);
+
+/*!*************************************************************************
+* wmt_codec_msvd_reset
+*
+* Public Function
+*/
+/*!
+* \brief
+*
+* \retval  0 if success
+*/
+int wmt_codec_msvd_reset(int codec_type)
+{
+	if (codec_type == CODEC_VD_MSVD) {
+		/* Do MSVD SW reset */
+		REG_WRITE(MSVD_BASE_ADDR + 0x008, 0x07);
+		REG_WRITE(MSVD_BASE_ADDR + 0x008, 0x00);
+
+		/* automatic disable clock , BIT0 AHB, BIT1 core, BIT2 NA bus*/
+		REG_WRITE(MSVD_BASE_ADDR + 0x004, 0x00);
+
+		/* Disable SRAM Power Down */
+		REG_WRITE(MSVD_BASE_ADDR + 0x00C, 0x00);
+	} else if (codec_type == CODEC_VD_JPEG) {
+		REG_WRITE(MSVD_BASE_ADDR + 0x028, 0xF); /* Enable MSVD SW reset */
+		REG_WRITE(MSVD_BASE_ADDR + 0x028, 0x0); /* Disable MSVD SW reset */
+		REG_WRITE(MSVD_BASE_ADDR + 0x02C, 0x0); /* Disable MSVD SW reset */
+	} else if (codec_type == CODEC_VE_H264) {
+		REG_WRITE(MSVD_BASE_ADDR + 0x048, 0xF); /* Enable MSVD SW reset */
+		REG_WRITE(MSVD_BASE_ADDR + 0x048, 0x0); /* Disable MSVD SW reset */
+		REG_WRITE(MSVD_BASE_ADDR + 0x044, 0xC); /* Enable Auto disable clock */
+		REG_WRITE(MSVD_BASE_ADDR + 0x04C, 0x0); /* Disable SRAM Power down */
+	} else if (codec_type == CODEC_VE_JPEG) {
+		REG_WRITE(MSVD_BASE_ADDR + 0x068, 0x0F);
+		REG_WRITE(MSVD_BASE_ADDR + 0x068, 0x00);
+		REG_WRITE(MSVD_BASE_ADDR + 0x06C, 0x00);
+	}
+	return 0;
+} /* End of wmt_codec_msvd_reset() */
+EXPORT_SYMBOL(wmt_codec_msvd_reset);
+
+/*!*************************************************************************
+* wmt_codec_lock
+*
+* Public Function
+*/
+/*!
+* \brief
+*
+* \retval  0 if success
+*/
+int wmt_codec_lock(int codec_type, int timeout)
+{
+	struct semaphore *sem;
+	int ret = ret;
+
+	if ((codec_type != CODEC_VD_JPEG) && (codec_type != CODEC_VE_JPEG))
+		return -1;
+
+	if (codec_lock[codec_type].is_init != 1) {
+		sema_init(&codec_lock[codec_type].sem, 1);
+
+		codec_lock[codec_type].is_init = 1;
+	}
+	sem = &codec_lock[codec_type].sem;
+
+	if (timeout == 0) {
+		ret = down_trylock(sem);
+		if (ret)
+			ret = -ETIME;  /* reasonable if lock holded by other */
+	} else if (timeout == -1) {
+		ret = down_interruptible(sem);
+		if (ret)
+			DBG_ERR("<%d> down_interruptible fail (ret: %d)\n",
+				codec_type, ret);
+	} else {
+		ret = down_timeout(sem, msecs_to_jiffies(timeout));
+		if (ret)
+			DBG_MSG("<%d> down_timeout(%d ms) fail (ret: %d)\n",
+				codec_type, timeout, ret);
+	}
+	if (ret == 0)
+		codec_lock[codec_type].is_locked = 1;
+	return ret;
+} /* End of wmt_codec_lock() */
+EXPORT_SYMBOL(wmt_codec_lock);
+
+/*!*************************************************************************
+* wmt_codec_unlock
+*
+* Public Function
+*/
+/*!
+* \brief
+*
+* \retval  0 if success
+*/
+int wmt_codec_unlock(int codec_type)
+{
+	if ((codec_type != CODEC_VD_JPEG) && (codec_type != CODEC_VE_JPEG))
+		return -1;
+
+	if (codec_lock[codec_type].is_locked == 1) {
+		up(&codec_lock[codec_type].sem);
+		codec_lock[codec_type].is_locked = 0;
+	} else {
+		DBG_ERR("Try to unlock non-locked sem (%s)",
+			(codec_type == CODEC_VD_JPEG) ? "jdec" : "jenc");
+		return -1;
+	}
+	return 0;
+} /* End of wmt_codec_unlock() */
+EXPORT_SYMBOL(wmt_codec_unlock);
+
+/*!*************************************************************************
+* wmt_codec_write_prd
+*
+* Public Function
+*/
+/*!
+* \brief
+*   Transfer the buffer address as PRD foramt
+*
+* \parameter
+*   prd_addr  [IN] Phyical address
+*
+* \retval  0 if success
+*/
+int wmt_codec_write_prd(
+	unsigned int src_buf,
+	unsigned int src_size,
+	unsigned int prd_addr_in,
+	unsigned int prd_buf_size)
+{
+	#define PRD_MAX_SIZE (60*1024)
+
+	unsigned char *buf_addr = (unsigned char *)src_buf;
+	unsigned int   buf_size = src_size;
+	unsigned int  *prd_addr = (unsigned int  *)prd_addr_in;
+	unsigned int   items;
+	unsigned int   count = 0;
+	/*----------------------------------------------------------------------
+		Transfer the input address as PRD foramt
+	----------------------------------------------------------------------*/
+	DBG_MSG("src_buf: 0x%x, src_size: %d, prd_addr_in: 0x%x\n",
+			src_buf, src_size, prd_addr_in);
+	items = prd_buf_size/8;
+	while (buf_size > 0) {
+		if (buf_size > PRD_MAX_SIZE) {
+			*prd_addr++ = (unsigned int)buf_addr;
+			*prd_addr++ = PRD_MAX_SIZE;
+			buf_size -= PRD_MAX_SIZE;
+			buf_addr += PRD_MAX_SIZE;
+		} else {
+			*prd_addr++ = (unsigned int)buf_addr;
+			*prd_addr++ = (0x80000000 | buf_size);
+			buf_size = 0;
+		}
+		count++;
+		if (count > items)
+			return -1;
+	}
+	return 0;
+} /* End of wmt_codec_write_prd() */
+EXPORT_SYMBOL(wmt_codec_write_prd);
+
+/*!*************************************************************************
+* wmt_codec_dump_prd
+*
+* Public Function
+*/
+/*!
+* \brief
+*   Dump data in PRD foramt for buffer
+*
+* \parameter
+*   prd_addr  [IN] Phyical address
+*
+* \retval  0 if success
+*/
+int wmt_codec_dump_prd(unsigned int prd_virt_in, int dump_data)
+{
+	unsigned int prd_addr_in, prd_data_size = 0;
+	unsigned int addr, len;
+	int i, j;
+
+	prd_addr_in = prd_virt_in;
+	for (i = 0; ; i += 2) {
+		addr = *(unsigned int *)(prd_addr_in + i * 4);
+		len  = *(unsigned int *)(prd_addr_in + (i + 1) * 4);
+		prd_data_size += (len & 0xFFFF);
+
+		PRINTK("[%02d]Addr: 0x%08x\n", i, addr);
+		PRINTK("    Len:  0x%08x (%d)\n", len, (len & 0xFFFF));
+
+		if (dump_data) {
+			unsigned char *ptr;
+
+			ptr = (unsigned char *)phys_to_virt(addr);
+			for (j = 0; j < (len & 0xFFFF); j++) {
+				if ((j%16) == 15)
+					PRINTK("0x%02x\n", *ptr);
+				else
+					PRINTK("0x%02x ", *ptr);
+				ptr++;
+			}
+		}
+		if (len & 0x80000000)
+			break;
+	} /* for(i=0; ; i+=2) */
+	PRINTK("Data size in PRD table: %d\n", prd_data_size);
+
+	return 0;
+} /* End of wmt_codec_dump_prd() */
+EXPORT_SYMBOL(wmt_codec_dump_prd);
+
+/*!*************************************************************************
+* check_debug_option
+*
+* Private Function
+*/
+/*!
+* \brief
+*   Initial VD timer
+*
+* \retval  0 if success
+*/
+#ifdef CFG_CODEC_PERFORM_EN
+static void check_debug_option(void)
+{
+	extern int wmt_getsyspara(char *varname, unsigned char *varval, int *varlen);
+
+	static int s_first_run;
+
+	if (s_first_run == 0) {
+		char buf[80] = {0};
+		int varlen = 80;
+
+		/* Read u-boot parameter to decide value of wmt_codec_debug */
+		if (wmt_getsyspara("wmt.codec.debug", buf, &varlen) == 0)
+			wmt_codec_debug = simple_strtol(buf, NULL, 10);
+
+		s_first_run = 1;
+	}
+} /* End of check_debug_option() */
+#endif /* #ifdef CFG_CODEC_PERFORM_EN */
+
+/*!*************************************************************************
+* wmt_codec_timer_init
+*
+* API Function
+*/
+/*!
+* \brief
+*   Initial VD timer
+*
+* \retval  0 if success
+*/
+int wmt_codec_timer_init(
+	void **pphandle,
+	const char *name,
+	unsigned int count,
+	int threshold_ms)
+{
+#ifdef CFG_CODEC_PERFORM_EN
+	static atomic_t s_Seq_id = ATOMIC_INIT(0);
+	struct wmt_tm *pcodec;
+
+	check_debug_option();
+
+	if (wmt_codec_debug == 0)
+		return 0;
+
+	pcodec = kmalloc(sizeof(struct wmt_tm), GFP_KERNEL);
+	if (pcodec == 0)
+		DBG_ERR("Allocate %d bytes fail\n", sizeof(struct wmt_tm));
+
+	memset(pcodec, 0, sizeof(struct wmt_tm));
+
+	pcodec->reset     = count;
+	pcodec->threshold = threshold_ms * 1000; /* us */
+	pcodec->max       = 0;
+	pcodec->min       = 0xFFFFFFF;
+	pcodec->initial   = 1;
+	strcpy(pcodec->name, name);
+
+	pcodec->seq_id = (unsigned long)(atomic_add_return(1, &s_Seq_id));
+
+	*pphandle = pcodec;
+#endif /* #ifdef CFG_CODEC_PERFORM_EN */
+	return 0;
+} /* End of wmt_codec_timer_init() */
+EXPORT_SYMBOL(wmt_codec_timer_init);
+
+/*!*************************************************************************
+* wmt_codec_timer_reset_count
+*
+* API Function
+*/
+/*!
+* \brief
+*   Release VD timer
+*
+* \retval  0 if success
+*/
+int wmt_codec_timer_reset(void *phandle, unsigned int count, int threshold_ms)
+{
+#ifdef CFG_CODEC_PERFORM_EN
+	struct wmt_tm *pcodec = (struct wmt_tm *)phandle;
+
+	if (pcodec) {
+		pcodec->reset     = count;
+		pcodec->threshold = threshold_ms * 1000; /* us */
+	}
+#endif /* #ifdef CFG_CODEC_PERFORM_EN */
+	return 0;
+} /* End of wmt_codec_timer_reset_count() */
+EXPORT_SYMBOL(wmt_codec_timer_reset);
+
+/*!*************************************************************************
+* wmt_codec_timer_start
+*
+* API Function
+*/
+/*!
+* \brief
+*   Start a VD timer
+*
+* \retval  0 if success
+*/
+int wmt_codec_timer_start(void *phandle)
+{
+#ifdef CFG_CODEC_PERFORM_EN
+	struct wmt_tm *pcodec = (struct wmt_tm *)phandle;
+
+	if (wmt_codec_debug == 0)
+		return 0;
+
+	if (pcodec->initial != 1) {
+		DBG_ERR("Timer was not initialized!\n");
+		return -1;
+	}
+	do_gettimeofday(&pcodec->start);
+#endif
+	return 0;
+} /* End of wmt_codec_timer_start()*/
+EXPORT_SYMBOL(wmt_codec_timer_start);
+
+/*!*************************************************************************
+* wmt_codec_timer_stop
+*
+* API Function
+*/
+/*!
+* \brief
+*   Stop a VD timer
+*
+* \retval  0 if success
+*/
+int wmt_codec_timer_stop(void *phandle)
+{
+#ifdef CFG_CODEC_PERFORM_EN
+	struct wmt_tm *pcodec = (struct wmt_tm *)phandle;
+	int this_time;
+
+	if (wmt_codec_debug == 0)
+		return 0;
+
+	if (pcodec->initial != 1) {
+		DBG_ERR("timer was not initialized!\n");
+		return -1;
+	}
+	do_gettimeofday(&pcodec->end);
+
+	/* unit in us */
+	if (pcodec->start.tv_sec == pcodec->end.tv_sec) {
+		this_time = pcodec->end.tv_usec - pcodec->start.tv_usec;
+	} else {
+		this_time = (pcodec->end.tv_sec - pcodec->start.tv_sec)*1000000
+			  + (pcodec->end.tv_usec - pcodec->start.tv_usec);
+	}
+	if (this_time < 0) {
+		PRINTK("Start sec: %ld, usec: %ld\n",
+			pcodec->start.tv_sec, pcodec->start.tv_usec);
+		PRINTK("End   sec: %ld, usec: %ld\n",
+			pcodec->end.tv_sec, pcodec->end.tv_usec);
+	}
+
+	pcodec->total_tm += this_time;
+	pcodec->interval_tm += this_time;
+	pcodec->total_cnt++;
+
+	if (this_time >= pcodec->max)
+		pcodec->max = this_time;
+	if (this_time <= pcodec->min)
+		pcodec->min = this_time;
+
+	if (pcodec->threshold && (this_time >  pcodec->threshold)) {
+		PRINTK("[%s] (%d) Decode time(%d) over %d (usec)\n",
+			pcodec->name, pcodec->total_cnt,
+			this_time, pcodec->threshold);
+	}
+	pcodec->count++;
+	if ((pcodec->reset != 0) && (pcodec->count >= pcodec->reset)) {
+		PRINTK("=================================================\n");
+		PRINTK("[%s] Avg. time = %d (usec)\n",
+			pcodec->name, pcodec->interval_tm/pcodec->count);
+		PRINTK("(~ %d) Decode Time Range[%d ~ %d](usec)\n",
+			pcodec->total_cnt, pcodec->min, pcodec->max);
+		pcodec->interval_tm = 0;
+		pcodec->count = 0;
+	}
+#endif /* #ifdef CFG_CODEC_PERFORM_EN */
+	return 0;
+} /* End of wmt_codec_timer_stop()*/
+EXPORT_SYMBOL(wmt_codec_timer_stop);
+
+/*!*************************************************************************
+* wmt_codec_timer_exit
+*
+* API Function
+*/
+/*!
+* \brief
+*   Release VD timer
+*
+* \retval  0 if success
+*/
+int wmt_codec_timer_exit(void *phandle)
+{
+#ifdef CFG_CODEC_PERFORM_EN
+	struct wmt_tm *pcodec = (struct wmt_tm *)phandle;
+
+	if (wmt_codec_debug == 0)
+		return 0;
+
+	if (pcodec == 0)
+		DBG_ERR("Illegal NULL handle!\n");
+
+	if (pcodec->initial != 1) {
+		DBG_ERR("Codec(%s) timer was not initialized!\n", pcodec->name);
+		return -1;
+	}
+	if (pcodec->total_cnt) {
+		unsigned int avg_tm = pcodec->total_tm/pcodec->total_cnt;
+		PRINTK("=== [seq_id: %ld %s] Timer status:\n",
+			pcodec->seq_id, pcodec->name);
+		PRINTK("Total count = %d\n", pcodec->total_cnt);
+		PRINTK("Total time  = %d (usec)\n", pcodec->total_tm);
+		PRINTK("Avg. time   = %d (usec)\n", avg_tm);
+		PRINTK("Max time    = %d (usec)\n", pcodec->max);
+		PRINTK("Min time    = %d (usec)\n", pcodec->min);
+		PRINTK("==========================================\n");
+	}
+	/* reset all */
+	memset(pcodec, 0, sizeof(struct wmt_tm));
+	kfree(pcodec);
+#endif /* #ifdef CFG_CODEC_PERFORM_EN */
+	return 0;
+} /* End of wmt_codec_timer_exit()*/
+EXPORT_SYMBOL(wmt_codec_timer_exit);
+
+MODULE_AUTHOR("WonderMedia Technologies, Inc.");
+MODULE_LICENSE("GPL");
+
+/*--------------------End of Function Body -----------------------------------*/
+#endif /* ifndef CODEC_C */
diff --git a/drivers/media/video/wmt/wmt-codec.h b/drivers/media/video/wmt/wmt-codec.h
new file mode 100644
index 00000000..86e62f20
--- /dev/null
+++ b/drivers/media/video/wmt/wmt-codec.h
@@ -0,0 +1,72 @@
+/*++
+ * Common interface for WonderMedia SoC hardware encoder and decoder drivers
+ *
+ * Copyright (c) 2008-2013  WonderMedia  Technologies, Inc.
+ *
+ * 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, see <http://www.gnu.org/licenses/>.
+ *
+ * WonderMedia Technologies, Inc.
+ * 4F, 533, Chung-Cheng Road, Hsin-Tien, Taipei 231, R.O.C
+--*/
+#ifndef CODEC_H
+#define CODEC_H
+
+/*-------------------- MODULE DEPENDENCY ---------------------------------*/
+
+enum {
+	CODEC_VD_JPEG,
+	CODEC_VD_MSVD,
+	CODEC_VE_H264,
+	CODEC_VE_JPEG,
+	CODEC_MAX
+}; /* codec_type */
+
+/*--------------------- EXPORTED PRIVATE FUNCTIONS  ----------------------*/
+
+int wmt_get_codec_clock_count(void);
+void wmt_reset_codec_clock_count(void);
+
+int wmt_codec_pmc_ctl(int codec_type, int enable);
+int wmt_codec_clock_en(int codec_type, int enable);
+int wmt_codec_msvd_reset(int codec_type);
+
+int wmt_codec_lock(int codec_type, int timeout);
+int wmt_codec_unlock(int codec_type);
+
+int wmt_codec_write_prd(
+	unsigned int src_buf,
+	unsigned int src_size,
+	unsigned int prd_addr_in,
+	unsigned int prd_buf_size);
+
+int wmt_codec_dump_prd(unsigned int prd_virt_in, int dump_data);
+
+int wmt_codec_timer_init(
+	void **ppcodec,
+	const char *name,
+	unsigned int count,
+	int threshold_ms);
+
+int wmt_codec_timer_reset(
+	void *phandle,
+	unsigned int count,
+	int threshold_ms);
+
+int wmt_codec_timer_start(void *pcodec);
+int wmt_codec_timer_stop(void  *pcodec);
+int wmt_codec_timer_exit(void  *pcodec);
+
+/*=== END wmt-codec.h ====================================================*/
+#endif /* #ifndef CODEC_H */
+
diff --git a/drivers/media/video/wmt/wmt-lock.c b/drivers/media/video/wmt/wmt-lock.c
new file mode 100644
index 00000000..7bf72ce9
--- /dev/null
+++ b/drivers/media/video/wmt/wmt-lock.c
@@ -0,0 +1,512 @@
+/*++
+ * WonderMedia Codec Lock driver
+ *
+ * Copyright (c) 2008-2013  WonderMedia  Technologies, Inc.
+ *
+ * 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, see <http://www.gnu.org/licenses/>.
+ *
+ * WonderMedia Technologies, Inc.
+ * 4F, 533, Chung-Cheng Road, Hsin-Tien, Taipei 231, R.O.C
+--*/
+
+#include <linux/fs.h>
+#include <linux/module.h>
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/cdev.h>
+#include <linux/pci.h>
+#include <asm/uaccess.h>
+#include <linux/proc_fs.h>
+#include <linux/sched.h>
+#include <linux/major.h>
+
+#include "com-lock.h"
+
+#define WMT_LOCK_NAME     "wmt-lock"
+#define WMT_LOCK_MAJOR    MB_MAJOR   /* same as memblock driver */
+#define WMT_LOCK_MINOR    1      /* 0 is /dev/mbdev, 1 is /dev/wmt-lock */
+
+#define MAX_LOCK_JDEC     1       /* Max lock number for JPEG decoder */
+
+/* Even for multi-decoding, this value should <= 4 */
+#define MAX_LOCK_VDEC     4
+
+#define MAX_LOCK_TYPE     3       /* JPEG, MSVD & encoders */
+
+#define MAX_LOCK_OWNER  (MAX_LOCK_JDEC + MAX_LOCK_VDEC)
+
+
+static struct class *wmt_lock_class;
+
+
+#undef WMT_LOCK_DEBUG
+/*#define WMT_LOCK_DEBUG*/
+
+#ifdef WMT_LOCK_DEBUG
+#define P_DEBUG(fmt, args...)    printk(KERN_INFO "["WMT_LOCK_NAME"] " fmt , ## args)
+#else
+#define P_DEBUG(fmt, args...)    ((void)(0))
+#endif
+
+#define P_INFO(fmt, args...)     printk(KERN_INFO "[wmt-lock] " fmt , ## args)
+#define P_WARN(fmt, args...)     printk(KERN_WARNING "[wmt-lock] *W* " fmt, ## args)
+#define P_ERROR(fmt, args...)    printk(KERN_ERR "[wmt-lock] *E* " fmt , ## args)
+
+
+static struct cdev wmt_lock_cdev;
+
+struct lock_owner {
+	pid_t  pid;
+	void  *private_data;
+	char   comm[TASK_COMM_LEN];
+	const char        *type;
+};
+
+struct semaphore lock_sem_jpeg;
+struct semaphore lock_sem_video;
+struct semaphore lock_sem_encoder;
+
+struct lock_owner_s {
+	const char        *type;
+	struct semaphore  *sem;
+	unsigned int       max_lock_num;
+	struct lock_owner  owners[MAX_LOCK_OWNER];
+};
+
+struct lock_owner_s gLockers[MAX_LOCK_TYPE];
+
+static spinlock_t   gSpinlock;
+
+
+/*!*************************************************************************
+* get_sema
+*
+* Private Function
+*
+* \brief
+*
+* \retval  ponters to specified semaphore
+*/
+static struct semaphore *get_sema(long lock_type)
+{
+	return gLockers[lock_type].sem;
+}
+
+/*!*************************************************************************
+* set_owner
+*
+* Private Function
+*
+* \brief
+*
+* \retval  0 if success, error code if fail
+*/
+static struct lock_owner *set_owner(long lock_type, struct file *filp)
+{
+	struct lock_owner *o;
+	int i;
+
+	for (i = 0; i < gLockers[lock_type].max_lock_num; i++) {
+		if (gLockers[lock_type].owners[i].pid == 0) {
+			o = &gLockers[lock_type].owners[i];
+			o->private_data = filp->private_data;
+			o->pid          = current->pid;
+			memcpy(o->comm, current->comm, TASK_COMM_LEN);
+			return o;
+		}
+	}
+	P_ERROR("Set owner fail because of %s lock full\n",
+		gLockers[lock_type].type);
+	return 0;
+}
+
+/*!*************************************************************************
+* get_owner
+*
+* Private Function
+*
+* \brief
+*
+* \retval  0 if success, error code if fail
+*/
+static struct lock_owner *get_owner(long lock_type)
+{
+	struct lock_owner *o;
+	int i;
+
+	for (i = 0; i < gLockers[lock_type].max_lock_num; i++) {
+		o = &gLockers[lock_type].owners[i];
+		if (o->pid == current->pid)
+			return o;
+	}
+	return NULL;
+}
+
+/*!*************************************************************************
+* check_busy
+*
+* Private Function
+*
+* \brief
+*
+* \retval  0 if not busy, otherwise return 1
+*/
+static int check_busy(long lock_type)
+{
+	struct lock_owner *o;
+	int i;
+
+	for (i = 0; i < gLockers[lock_type].max_lock_num; i++) {
+		o = &gLockers[lock_type].owners[i];
+		if (o->pid == current->pid)
+			return 1; /* Busy */
+	}
+	return 0;
+}
+
+/*!*************************************************************************
+* lock_read_proc
+*
+* Private Function
+*
+* \brief
+*
+* \retval  0 if success, error code if fail
+*/
+static int lock_read_proc(
+	char *page, char **start,
+	off_t offset,
+	int len,
+	int *eof,
+	void *data)
+{
+	char *p = page;
+	struct lock_owner *o;
+	int i, j;
+
+	for (i = 0; i < MAX_LOCK_TYPE; i++) {
+		p += sprintf(p, "------ %s lock ------\n", gLockers[i].type);
+		for (j = 0; j < gLockers[i].max_lock_num; j++) {
+			o = &gLockers[i].owners[j];
+			if (o->pid != 0)
+				p += sprintf(p, "%s lock : occupied by [%s,%d]\n",
+					gLockers[i].type, o->comm, o->pid);
+			else
+				p += sprintf(p, "%s lock : Free\n", gLockers[i].type);
+		}
+	}
+	return p - page;
+}
+
+/*!*************************************************************************
+* wmt_lock_ioctl
+*
+* Private Function
+*
+* \brief
+*
+* \retval  0 if success, error code if fail
+*/
+static long wmt_lock_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
+{
+	int ret = 0;
+	struct lock_owner *o;
+	struct semaphore *sem;
+	struct wmt_lock *lock_arg;
+	long timeout;
+	unsigned long flags = 0;
+
+	/* check ioctl type and number, if fail return EINVAL */
+	if (_IOC_TYPE(cmd) != LOCK_IOC_MAGIC) {
+		P_WARN("ioctl unknown cmd %X, type %X by [%s,%d]\n",
+			cmd, _IOC_TYPE(cmd), current->comm, current->pid);
+		return -EINVAL;
+	}
+
+	if (!access_ok(VERIFY_READ, (void __user *)arg, sizeof(struct wmt_lock))) {
+		P_WARN("ioctl access_ok failed, cmd %X, type %X by [%s,%d]\n",
+			cmd, _IOC_TYPE(cmd), current->comm, current->pid);
+		return -EFAULT;
+	}
+
+	lock_arg = (struct wmt_lock *)arg;
+
+	if (lock_arg->lock_type >= MAX_LOCK_TYPE || lock_arg->lock_type < 0) {
+		P_WARN("invalid lock type %ld by [%s,%d]\n",
+			lock_arg->lock_type, current->comm, current->pid);
+		return -E2BIG;
+	}
+
+	spin_lock_irqsave(&gSpinlock, flags);
+	sem  = get_sema(lock_arg->lock_type);
+
+	switch (cmd) {
+	case IO_WMT_LOCK:
+		timeout = lock_arg->arg2;
+		/* check if the current thread already get the lock */
+		if (check_busy(lock_arg->lock_type)) {
+			P_WARN("Recursive %s lock by [%s,%d]\n",
+				gLockers[lock_arg->lock_type].type,
+				current->comm, current->pid);
+			return -EBUSY;
+		}
+
+		if (timeout == 0) {
+			ret = down_trylock(sem);
+			if (ret)
+				ret = -ETIME;  /* reasonable if lock holded by other */
+		} else if (timeout == -1) {
+			ret = down_interruptible(sem);
+			if (ret)
+				P_INFO("Require %s lock error %d by [%s,%d]\n",
+					gLockers[lock_arg->lock_type].type, ret,
+					current->comm, current->pid);
+		} else {
+			/* require lock with a timeout value, please beware
+				this function can't exit when interrupt */
+			spin_unlock_irqrestore(&gSpinlock, flags);
+			ret = down_timeout(sem, msecs_to_jiffies(timeout));
+			spin_lock_irqsave(&gSpinlock, flags);
+		}
+		if (ret == 0) {
+			o = set_owner(lock_arg->lock_type, filp);
+			if (o)
+				P_DEBUG("%s is locked by [%s,%d], seq %d\n",
+					o->type, o->comm, o->pid, (int)filp->private_data);
+		}
+		break;
+
+	case IO_WMT_UNLOCK:
+		o = get_owner(lock_arg->lock_type);
+		if (o == NULL) {
+			P_WARN("Unnecessary %s unlock from [%s,%d] when lock is free\n",
+				gLockers[lock_arg->lock_type].type,
+				current->comm, current->pid);
+			ret = -EACCES;
+		} else if (filp->private_data == o->private_data) {
+			P_DEBUG("%s is unlocked by [%s,%d], seq %d\n",
+				o->type, o->comm, o->pid, (int)filp->private_data);
+			o->pid = 0;
+			o->private_data = 0;
+			up(sem);
+			ret = 0;
+		} else{
+			P_WARN("Unexpected %s unlock from [%s,%d], hold by [%s,%d] now\n",
+				o->type, current->comm, current->pid, o->comm, o->pid);
+			ret = -EACCES;
+		}
+		break;
+
+	case IO_WMT_CHECK_WAIT:
+		if (sem->count > 0)
+			lock_arg->is_wait = 0;
+		else {
+			if ((sem->wait_list.prev == 0) && (sem->wait_list.next == 0))
+				lock_arg->is_wait = 0;
+			else
+				lock_arg->is_wait = 1;
+		}
+		P_INFO("sem->count: %d\n", sem->count);
+		P_INFO("lock_arg->is_wait: %d\n", lock_arg->is_wait);
+		break;
+
+	default:
+		P_WARN("ioctl unknown cmd 0x%X  by [%s,%d]\n",
+			cmd, current->comm, current->pid);
+		ret = -EINVAL;
+	}
+	spin_unlock_irqrestore(&gSpinlock, flags);
+
+	return ret;
+}
+
+/*!*************************************************************************
+* wmt_lock_open
+*
+* Private Function
+*
+* \brief
+*
+* \retval  0 if success, error code if fail
+*/
+static int wmt_lock_open(struct inode *inode, struct file *filp)
+{
+	static atomic_t lock_seq_id = ATOMIC_INIT(0);
+
+	/* use a sequence number as the file open id */
+	filp->private_data = (void *)(atomic_add_return(1, &lock_seq_id));
+	P_DEBUG("open by [%s,%d], seq %d\n",
+		current->comm, current->pid, (int)filp->private_data);
+	return 0;
+}
+
+/*!*************************************************************************
+* wmt_lock_release
+*
+* Private Function
+*
+* \brief
+*
+* \retval  0 if success, error code if fail
+*/
+static int wmt_lock_release(struct inode *inode, struct file *filp)
+{
+	int i, j;
+
+	for (i = 0; i < MAX_LOCK_TYPE; i++) {
+		for (j = 0; j < gLockers[i].max_lock_num; j++) {
+			struct lock_owner *o = &gLockers[i].owners[j];
+			if (o->pid != 0 && filp->private_data == o->private_data) {
+				P_WARN("Auto free %s lock hold by [%s,%d]\n",
+					o->type, o->comm, o->pid);
+				o->pid = 0;
+				o->private_data = 0;
+				up(get_sema(i));
+			}
+		}
+	}
+	P_DEBUG("Release by [%s,%d], seq %d\n",
+		current->comm, current->pid, (int)filp->private_data);
+	return 0;
+}
+
+static const struct file_operations wmt_lock_fops = {
+	.owner   = THIS_MODULE,
+	.open    = wmt_lock_open,
+	.unlocked_ioctl = wmt_lock_ioctl,
+	.release = wmt_lock_release,
+};
+
+static void check_multi_vd_count(int *max_vd_count)
+{
+	extern int wmt_getsyspara(char *varname, unsigned char *varval, int *varlen);
+
+	char buf[80] = {0};
+	int varlen = 80;
+	int max_count = 1;
+
+	/* Read u-boot parameter to decide value of wmt_codec_debug */
+	/*----------------------------------------------------------------------
+		Check wmt.codec.debug
+	----------------------------------------------------------------------*/
+	if (wmt_getsyspara("wmt.multi.vd.max", buf, &varlen) == 0)
+		max_count = simple_strtol(buf, NULL, 10);
+
+	if (max_count < 1)
+		max_count = 1;
+	if (max_count > MAX_LOCK_VDEC)
+		max_count = MAX_LOCK_VDEC;
+
+	*max_vd_count = max_count;
+
+	return;
+} /* End of check_debug_option() */
+
+/*!*************************************************************************
+* wmt_lock_init
+*
+* Private Function
+*
+* \brief
+*
+* \retval  0 if success, error code if fail
+*/
+static int __init wmt_lock_init(void)
+{
+	dev_t   dev_id;
+	int max_vd_count;
+	int ret, i, j;
+
+	dev_id = MKDEV(WMT_LOCK_MAJOR, WMT_LOCK_MINOR);
+	ret = register_chrdev_region(dev_id, 1, WMT_LOCK_NAME);
+	if (ret < 0) {
+		P_ERROR("can't register %s device %d:%d, ret %d\n",
+			WMT_LOCK_NAME, WMT_LOCK_MAJOR, WMT_LOCK_MINOR, ret);
+		return ret;
+	}
+
+	cdev_init(&wmt_lock_cdev, &wmt_lock_fops);
+	ret = cdev_add(&wmt_lock_cdev, dev_id, 1);
+	if (ret) {
+		P_ERROR("cdev add error(%d).\n", ret);
+		unregister_chrdev_region(dev_id, 1);
+		return ret;
+	}
+
+	/* let udev to handle /dev/wmt-lock */
+	wmt_lock_class = class_create(THIS_MODULE, WMT_LOCK_NAME);
+	device_create(wmt_lock_class, NULL, dev_id, NULL, "%s", WMT_LOCK_NAME);
+
+	create_proc_read_entry(WMT_LOCK_NAME, 0, NULL, lock_read_proc, NULL);
+	P_INFO("init ok, major=%d, minor=%d\n", WMT_LOCK_MAJOR, WMT_LOCK_MINOR);
+
+	spin_lock_init(&gSpinlock);
+
+	check_multi_vd_count(&max_vd_count);
+
+	/* Init sema for JPEG decoder */
+	sema_init(&lock_sem_jpeg, MAX_LOCK_JDEC);
+	gLockers[lock_jpeg].type = "jdec";
+	gLockers[lock_jpeg].sem = &lock_sem_jpeg;
+	gLockers[lock_jpeg].max_lock_num = MAX_LOCK_JDEC;
+
+	/* Init sema for MSVD decoders */
+	sema_init(&lock_sem_video, max_vd_count);
+	gLockers[lock_video].type = "vdec";
+	gLockers[lock_video].sem = &lock_sem_video;
+	gLockers[lock_video].max_lock_num = max_vd_count;
+
+	for (i = 0; i < MAX_LOCK_TYPE; i++) {
+		for (j = 0; j < gLockers[i].max_lock_num; j++) {
+			struct lock_owner *o = &gLockers[i].owners[j];
+			o->type = gLockers[i].type;
+		}
+	}
+	return ret;
+}
+
+/*!*************************************************************************
+* wmt_lock_cleanup
+*
+* Private Function
+*
+* \brief
+*
+* \retval  0 if success, error code if fail
+*/
+static void __exit wmt_lock_cleanup(void)
+{
+	dev_t dev_id = MKDEV(WMT_LOCK_MAJOR, WMT_LOCK_MINOR);
+
+	cdev_del(&wmt_lock_cdev);
+
+	/* let udev to handle /dev/wmt-lock */
+	device_destroy(wmt_lock_class, dev_id);
+	class_destroy(wmt_lock_class);
+
+	unregister_chrdev_region(dev_id, 1);
+	remove_proc_entry(WMT_LOCK_NAME, NULL);
+
+	P_INFO("cleanup done\n");
+}
+
+module_init(wmt_lock_init);
+module_exit(wmt_lock_cleanup);
+
+
+MODULE_AUTHOR("WonderMedia Technologies, Inc.");
+MODULE_DESCRIPTION("WMT Codec Lock device driver");
+MODULE_LICENSE("GPL");
+
diff --git a/drivers/media/video/wmt/wmt-lock.h b/drivers/media/video/wmt/wmt-lock.h
new file mode 100644
index 00000000..cf15aee3
--- /dev/null
+++ b/drivers/media/video/wmt/wmt-lock.h
@@ -0,0 +1,45 @@
+/*++ 
+ * Common interface for WonderMedia SoC hardware decoder drivers
+ *
+ * Copyright (c) 2008-2013  WonderMedia  Technologies, Inc.
+ *
+ * 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, see <http://www.gnu.org/licenses/>.
+ *
+ * WonderMedia Technologies, Inc.
+ * 4F, 533, Chung-Cheng Road, Hsin-Tien, Taipei 231, R.O.C
+--*/
+#ifndef WMTLOCK_H
+#define WMTLOCK_H
+
+/*-------------------- MODULE DEPENDENCY -------------------------------------*/
+
+/* Even for multi-decoding, this value should <= 4 */
+#define MAX_LOCK_NUM_VDEC   4            /* Max lock number for MSVD decoders */
+
+
+
+/*-------------------- EXPORTED PRIVATE VARIABLES -----------------------------*/
+#ifdef WMTLOCK_C
+    #define EXTERN
+#else
+    #define EXTERN   extern
+#endif /* ifdef WMTLOCK_C */
+
+#undef EXTERN
+
+/*--------------------- EXPORTED PRIVATE FUNCTIONS  ---------------------------*/
+
+/*=== END wmt-lock.h ==========================================================*/
+#endif /* #ifndef WMTLOCK_H */
+
diff --git a/drivers/media/video/wmt/wmt-vd.c b/drivers/media/video/wmt/wmt-vd.c
new file mode 100644
index 00000000..435c375c
--- /dev/null
+++ b/drivers/media/video/wmt/wmt-vd.c
@@ -0,0 +1,541 @@
+/*++
+ * Common interface for WonderMedia SoC hardware decoder drivers
+ *
+ * Copyright (c) 2008-2013  WonderMedia  Technologies, Inc.
+ *
+ * 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, see <http://www.gnu.org/licenses/>.
+ *
+ * WonderMedia Technologies, Inc.
+ * 4F, 533, Chung-Cheng Road, Hsin-Tien, Taipei 231, R.O.C
+--*/
+
+#include "wmt-vd.h"
+
+#define THE_MB_USER "WMT-VD"
+
+#define DRIVER_NAME "wmt-vd"
+
+#define VD_DEV_NAME "wmt-vd"
+
+/*#define VD_DEBUG_DEF*/
+/*#define VD_WORDY_DEF*/
+
+
+#define VD_INFO(fmt, args...) \
+	do {\
+		printk(KERN_INFO "[wmt-vd] " fmt , ## args);\
+	} while (0)
+
+#define VD_WARN(fmt, args...) \
+	do {\
+		printk(KERN_WARNING "[wmt-vd] " fmt , ## args);\
+	} while (0)
+
+#define VD_ERROR(fmt, args...) \
+	do {\
+		printk(KERN_ERR "[wmt-vd] " fmt , ## args);\
+	} while (0)
+
+#ifdef VD_DEBUG_DEF
+#define VD_DBG(fmt, args...) \
+	do {\
+		printk(KERN_DEBUG "[wmt-vd] %s: " fmt, __func__ , ## args);\
+	} while (0)
+#else
+#define VD_DBG(fmt, args...)
+#undef VD_WORDY_DEF
+#endif
+
+#ifdef VD_WORDY_DEF
+#define VD_WDBG(fmt, args...)	VD_DBG(fmt, args...)
+#else
+#define VD_WDBG(fmt, args...)
+#endif
+
+
+static struct class *vd_class;
+static int videodecoder_minor;
+static int videodecoder_dev_nr = 1;
+static struct cdev *videodecoder_cdev;
+
+static struct videodecoder *decoders[VD_MAX] = {0};
+
+static struct vd_resource vd_res = {
+	.prdt         = { NULL, 0x0, MAX_INPUT_BUF_SIZE},
+};
+
+static DEFINE_SEMAPHORE(vd_sem);
+
+static int wmt_vd_open(struct inode *inode, struct file *filp)
+{
+	int ret = -EINVAL;
+	unsigned int idx = VD_MAX;
+
+	down(&vd_sem);
+	idx = iminor(inode);
+
+	if (idx < VD_MAX && decoders[idx]) {
+		struct videodecoder *vd = decoders[idx];
+
+		if (vd && vd->fops.open) {
+			if (idx == VD_JPEG)
+				filp->private_data = (void *)&(vd_res.prdt);
+			ret = vd->fops.open(inode, filp);
+		}
+		VD_DBG("decoder %s opened.\n", vd->name);
+	}
+	up(&vd_sem);
+
+	return ret;
+} /* End of wmt_vd_open() */
+
+static int wmt_vd_release(struct inode *inode, struct file *filp)
+{
+	int ret = -EINVAL;
+	unsigned int idx = VD_MAX;
+
+	down(&vd_sem);
+	idx = iminor(inode);
+
+	if (idx < VD_MAX && decoders[idx]) {
+		struct videodecoder *vd = decoders[idx];
+		if (vd && vd->fops.release)
+			ret = vd->fops.release(inode, filp);
+		VD_DBG("decoder %s closed.\n", vd->name);
+	}
+	up(&vd_sem);
+
+	return ret;
+} /* End of wmt_vd_release() */
+
+static long wmt_vd_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
+{
+	int ret = -EINVAL;
+	unsigned int idx = VD_MAX;
+	struct inode *inode = filp->f_mapping->host;
+
+	/* check type and number, if fail return ENOTTY */
+	if ((_IOC_TYPE(cmd) != VD_IOC_MAGIC) || (_IOC_NR(cmd) > VD_IOC_MAXNR))
+		return -ENOTTY;
+
+	/* check argument area */
+	if (_IOC_DIR(cmd) & _IOC_READ)
+		ret = !access_ok(VERIFY_WRITE, (void __user *)arg,
+						_IOC_SIZE(cmd));
+	else if (_IOC_DIR(cmd) & _IOC_WRITE)
+		ret = !access_ok(VERIFY_READ, (void __user *)arg,
+						_IOC_SIZE(cmd));
+
+	if (ret)
+		return -EFAULT;
+
+	down(&vd_sem);
+	idx = iminor(inode);
+	if (idx < VD_MAX && decoders[idx]) {
+		struct videodecoder *vd = decoders[idx];
+
+		if (vd && vd->fops.unlocked_ioctl)
+			ret = vd->fops.unlocked_ioctl(filp, cmd, arg);
+	}
+	up(&vd_sem);
+
+	return ret;
+} /* End of wmt_vd_ioctl() */
+
+static int wmt_vd_mmap(struct file *filp, struct vm_area_struct *vma)
+{
+	int ret = -EINVAL;
+	unsigned int idx = VD_MAX;
+
+	down(&vd_sem);
+	if (filp && filp->f_dentry && filp->f_dentry->d_inode)
+		idx = iminor(filp->f_dentry->d_inode);
+
+	if (idx < VD_MAX && decoders[idx]) {
+		struct videodecoder *vd = decoders[idx];
+		if (vd && vd->fops.mmap)
+			ret = vd->fops.mmap(filp, vma);
+	}
+	up(&vd_sem);
+
+	return ret;
+}
+
+const struct file_operations videodecoder_fops = {
+	.owner          = THIS_MODULE,
+	.open           = wmt_vd_open,
+	.release        = wmt_vd_release,
+	.unlocked_ioctl = wmt_vd_ioctl,
+	.mmap           = wmt_vd_mmap,
+};
+
+static int wmt_vd_probe(struct platform_device *dev)
+{
+	dev_t dev_no;
+	int ret;
+
+	dev_no = MKDEV(VD_MAJOR, videodecoder_minor);
+
+	/* register char device */
+	videodecoder_cdev = cdev_alloc();
+	if (!videodecoder_cdev) {
+		VD_ERROR("alloc dev error.\n");
+		return -ENOMEM;
+	}
+
+	cdev_init(videodecoder_cdev, &videodecoder_fops);
+	ret = cdev_add(videodecoder_cdev, dev_no, 1);
+
+	if (ret) {
+		VD_ERROR("reg char dev error(%d).\n", ret);
+		cdev_del(videodecoder_cdev);
+		return ret;
+	}
+
+	vd_res.prdt.virt = dma_alloc_coherent(NULL, vd_res.prdt.size,
+				&vd_res.prdt.phys, GFP_KERNEL | GFP_DMA);
+	if (!vd_res.prdt.virt) {
+		VD_ERROR("allocate video PRDT buffer error.\n");
+		cdev_del(videodecoder_cdev);
+		vd_res.prdt.virt = NULL;
+		vd_res.prdt.phys = 0;
+		return -ENOMEM;
+	}
+
+	VD_DBG("prob /dev/%s major %d, minor %d, Resource:\n",
+		DRIVER_NAME, VD_MAJOR, videodecoder_minor);
+
+	VD_DBG("PRDT %p/%x, size %d KB\n",
+		vd_res.prdt.virt, vd_res.prdt.phys, vd_res.prdt.size/1024);
+
+	return ret;
+}
+
+static int wmt_vd_remove(struct platform_device *dev)
+{
+	unsigned int idx = 0;
+
+	down(&vd_sem);
+	while (idx < VD_MAX) {
+		if (decoders[idx] && decoders[idx]->remove) {
+			decoders[idx]->remove();
+			decoders[idx] = NULL;
+		}
+		idx++;
+	}
+	up(&vd_sem);
+
+	if (vd_res.prdt.virt)
+		dma_free_coherent(NULL, MAX_INPUT_BUF_SIZE,
+			vd_res.prdt.virt, vd_res.prdt.phys);
+
+	return 0;
+}
+
+static int wmt_vd_suspend(struct platform_device *dev, pm_message_t state)
+{
+	int ret;
+	unsigned int idx = 0;
+
+	down(&vd_sem);
+	while (idx < VD_MAX) {
+		if (decoders[idx] && decoders[idx]->suspend) {
+			ret = decoders[idx]->suspend(state);
+			if (ret < 0)
+				VD_WARN("vdec %s suspend fail. ret:%d\n",
+					decoders[idx]->name, ret);
+		}
+		idx++;
+	}
+	up(&vd_sem);
+
+	return 0;
+}
+
+static int wmt_vd_resume(struct platform_device *dev)
+{
+	int ret;
+	unsigned int idx = 0;
+
+	down(&vd_sem);
+	while (idx < VD_MAX) {
+		if (decoders[idx] && decoders[idx]->resume) {
+			ret = decoders[idx]->resume();
+			if (ret < 0) {
+				VD_WARN("vdec %s resume fail. ret:%d\n",
+					decoders[idx]->name, ret);
+			}
+		}
+		idx++;
+	}
+	up(&vd_sem);
+
+	return 0;
+}
+
+static void wmt_vd_elease(struct device *device)
+{
+	unsigned int idx = 0;
+
+	down(&vd_sem);
+	while (idx < VD_MAX) {
+		if (decoders[idx] && decoders[idx]->remove) {
+			decoders[idx]->remove();
+			decoders[idx] = NULL;
+		}
+		idx++;
+	}
+	up(&vd_sem);
+
+	if (vd_res.prdt.virt)
+		dma_free_coherent(NULL, MAX_INPUT_BUF_SIZE,
+			vd_res.prdt.virt, vd_res.prdt.phys);
+
+	return;
+}
+
+static struct platform_driver videodecoder_driver = {
+	.driver = {
+	.name           = "wmt-vd",
+	.bus            = &platform_bus_type,
+	},
+	.probe          = wmt_vd_probe,
+	.remove         = wmt_vd_remove,
+	.suspend        = wmt_vd_suspend,
+	.resume         = wmt_vd_resume
+};
+
+static struct platform_device videodecoder_device = {
+	.name           = "wmt-vd",
+	.id             = 0,
+	.dev            =   {
+						.release =  wmt_vd_elease,
+						},
+	.num_resources  = 0,        /* ARRAY_SIZE(spi_resources), */
+	.resource       = NULL,     /* spi_resources, */
+};
+
+#ifdef CONFIG_PROC_FS
+static int wmt_vd_read_proc(
+	char *page,
+	char **start,
+	off_t off,
+	int count,
+	int *eof,
+	void *data)
+{
+	int size;
+	unsigned int idx = 0;
+	char *p;
+
+	down(&vd_sem);
+	p = page;
+	p += sprintf(p, "***** video decoder information *****\n");
+
+	while (idx < VD_MAX) {
+		if (decoders[idx] && decoders[idx]->get_info) {
+			size = decoders[idx]->get_info(p, start, off, count);
+			count -= size;
+			if (count <= 40)
+				break;
+			p += size;
+		}
+		idx++;
+	}
+	p += sprintf(p, "**************** end ****************\n");
+	up(&vd_sem);
+
+	*eof = 1;
+
+	return p - page;
+}
+#endif
+
+int wmt_vd_register(struct videodecoder *dec)
+{
+	int ret = 0;
+	dev_t dev_no;
+
+	if (!dec || dec->id >= VD_MAX) {
+		VD_WARN("register invalid video decoder\n");
+		return -1;
+	}
+
+	if (decoders[dec->id]) {
+		VD_WARN("video decoder (ID=%d) exist.(E:%32s, N:%32s)\n",
+			dec->id, decoders[dec->id]->name, dec->name);
+		return -1;
+	}
+
+	dev_no = MKDEV(VD_MAJOR, dec->id);
+	ret = register_chrdev_region(dev_no, videodecoder_dev_nr, dec->name);
+	if (ret < 0) {
+		VD_ERROR("can't get %s device minor %d\n", dec->name, dec->id);
+		return ret;
+	}
+
+	dec->device = cdev_alloc();
+	if (!dec->device) {
+		unregister_chrdev_region(dev_no, videodecoder_dev_nr);
+		VD_ERROR("alloc dev error\n");
+		return -ENOMEM;
+	}
+
+	cdev_init(dec->device, &videodecoder_fops);
+	ret = cdev_add(dec->device, dev_no, 1);
+	if (ret) {
+		VD_ERROR("reg char dev error(%d)\n", ret);
+		unregister_chrdev_region(dev_no, videodecoder_dev_nr);
+		cdev_del(dec->device);
+		dec->device = NULL;
+		return ret;
+	}
+
+	if (dec->irq_num && dec->isr) {
+		ret = request_irq(dec->irq_num, dec->isr,
+				IRQF_SHARED, dec->name, dec->isr_data);
+		VD_INFO("%s Request IRQ %d %s.\n",
+			dec->name, dec->irq_num, (ret < 0) ? "Fail " : "Ok");
+		if (ret) {
+			VD_ERROR("isr: 0x%p, isr_data: 0x%p  (ret: %d)\n",
+				dec->isr, dec->isr_data, ret);
+			goto EXIT_wmt_vd_register;
+		}
+	}
+
+	if (!ret) {
+		if (dec->setup)
+			ret = dec->setup();
+	}
+
+	if (ret >= 0) {
+		down(&vd_sem);
+		decoders[dec->id] = dec;
+		up(&vd_sem);
+		VD_INFO("%s registered major %d minor %d\n",
+			dec->name, VD_MAJOR, dec->id);
+
+		/* let udev to handle /dev/ node */
+		dec->vd_class = class_create(dec->fops.owner, dec->name);
+		device_create(dec->vd_class, 0, dev_no, 0, "%s", dec->name);
+	} else {
+		VD_ERROR("%s register major %d minor %d fail\n",
+			dec->name, VD_MAJOR, dec->id);
+		free_irq(dec->irq_num, (void *)dec->isr_data);
+		unregister_chrdev_region(dev_no, videodecoder_dev_nr);
+		cdev_del(dec->device);
+		dec->device = NULL;
+	}
+EXIT_wmt_vd_register:
+	return ret;
+}
+EXPORT_SYMBOL(wmt_vd_register);
+
+int wmt_vd_unregister(struct videodecoder *dec)
+{
+	int ret = 0;
+	dev_t dev_no;
+
+	if (!dec || dec->id >= VD_MAX || !dec->device) {
+		VD_WARN("unregister invalid video decoder\n");
+		return -1;
+	}
+
+	if (decoders[dec->id] != dec) {
+		VD_WARN("unregiseter wrong video decoder. (E:%32s, R:%32s)\n",
+			decoders[dec->id]->name, dec->name);
+		return -1;
+	}
+
+	down(&vd_sem);
+	decoders[dec->id] = NULL;
+	up(&vd_sem);
+
+	if (dec->remove)
+		ret = dec->remove();
+
+	dev_no = MKDEV(VD_MAJOR, dec->id);
+	free_irq(dec->irq_num, (void *)dec->isr_data);
+
+	/* let udev to handle /dev/ node */
+	device_destroy(dec->vd_class, dev_no);
+	class_destroy(dec->vd_class);
+
+	unregister_chrdev_region(dev_no, videodecoder_dev_nr);
+	cdev_del(dec->device);
+	dec->device = NULL;
+
+	return ret;
+}
+EXPORT_SYMBOL(wmt_vd_unregister);
+
+static int __init videodecoder_init(void)
+{
+	int ret;
+	dev_t dev_no;
+
+	dev_no = MKDEV(VD_MAJOR, videodecoder_minor);
+	ret = register_chrdev_region(dev_no, videodecoder_dev_nr, "wmt-vd");
+	if (ret < 0) {
+		VD_ERROR("can't get %s major %d\n", DRIVER_NAME, VD_MAJOR);
+		return ret;
+	}
+
+#ifdef CONFIG_PROC_FS
+	create_proc_read_entry("wmt-vd", 0, NULL, wmt_vd_read_proc, NULL);
+	VD_DBG("create video decoder proc\n");
+#endif
+
+	ret = platform_driver_register(&videodecoder_driver);
+	if (!ret) {
+		ret = platform_device_register(&videodecoder_device);
+		if (ret)
+			platform_driver_unregister(&videodecoder_driver);
+	}
+
+	/* let udev to handle /dev/wmt-vd */
+	vd_class = class_create(THIS_MODULE, VD_DEV_NAME);
+	device_create(vd_class, NULL, dev_no, NULL, "%s", VD_DEV_NAME);
+
+	VD_INFO("WonderMedia HW decoder driver inited\n");
+
+	return ret;
+}
+
+void __exit videodecoder_exit(void)
+{
+	dev_t dev_no;
+
+	platform_driver_unregister(&videodecoder_driver);
+	platform_device_unregister(&videodecoder_device);
+	dev_no = MKDEV(VD_MAJOR, videodecoder_minor);
+
+	/* let udev to handle /dev/wmt-vd */
+	device_destroy(vd_class, dev_no);
+	class_destroy(vd_class);
+
+	unregister_chrdev_region(dev_no, videodecoder_dev_nr);
+
+	VD_INFO("WonderMedia HW decoder driver exit\n");
+
+	return;
+}
+
+fs_initcall(videodecoder_init);
+module_exit(videodecoder_exit);
+
+MODULE_AUTHOR("WonderMedia Technologies, Inc.");
+MODULE_DESCRIPTION("Video Codec device driver");
+MODULE_LICENSE("GPL");
+
diff --git a/drivers/media/video/wmt/wmt-vd.h b/drivers/media/video/wmt/wmt-vd.h
new file mode 100644
index 00000000..a981650b
--- /dev/null
+++ b/drivers/media/video/wmt/wmt-vd.h
@@ -0,0 +1,106 @@
+/*++
+ * Common interface for WonderMedia SoC hardware decoder drivers
+ *
+ * Copyright (c) 2008-2013  WonderMedia  Technologies, Inc.
+ *
+ * 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, see <http://www.gnu.org/licenses/>.
+ *
+ * WonderMedia Technologies, Inc.
+ * 4F, 533, Chung-Cheng Road, Hsin-Tien, Taipei 231, R.O.C
+--*/
+#ifndef WMT_VD_H
+#define WMT_VD_H
+
+/*-------------------- MODULE DEPENDENCY -------------------------------------*/
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/fs.h>
+#include <linux/kernel.h>
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/errno.h>
+#include <asm/uaccess.h>
+#include <linux/kdev_t.h>
+#include <linux/cdev.h>
+#include <linux/major.h>
+#include <linux/interrupt.h>
+#include <linux/spinlock.h>
+#include <linux/mm.h>
+#include <asm/page.h>
+
+
+#include <linux/dma-mapping.h>
+#include <linux/irqreturn.h>
+
+#include <mach/irqs.h>
+#include <mach/hardware.h>
+#include <linux/sched.h>     /* for 2.6.32 wait_event_interruptible() */
+
+#include <linux/wmt-mb.h>
+
+#ifdef CONFIG_PROC_FS
+#include <linux/proc_fs.h>
+    #include <linux/uaccess.h>
+#endif
+
+#include "com-vd.h"
+
+
+/*-------------------- EXPORTED PRIVATE CONSTANTS ------------------------*/
+
+/*--------------------------------------------------------------------------
+	Since the page size in kernel is 4 KB, so we may assume the max buffer
+	size as input buffer size = (prd_size/8)*4KB
+	In short, 1 KB PRD size could store about 0.5 MB data.
+	If we support maximun input buffer size is 50 MB, we must set prd_size
+	as 100 KB.
+---------------------------------------------------------------------------*/
+#define MAX_INPUT_BUF_SIZE  (100*1024) /* 100 KB */
+
+struct vd_mem_set {
+	void            *virt;
+	dma_addr_t      phys;
+	unsigned int    size;
+};
+
+struct vd_resource {
+	struct vd_mem_set prdt;
+	struct vd_mem_set cnm_fw_real;
+	struct vd_mem_set cnm_fw_align;
+};
+
+struct videodecoder {
+	char name[32];
+	int id;
+	int (*setup)(void);
+	int (*remove)(void);
+	int (*suspend)(pm_message_t state);
+	int (*resume)(void);
+	const struct file_operations  fops;
+	int (*get_info)(char *, char **, off_t, int);
+	struct cdev *device;
+	int irq_num;
+	irqreturn_t (*isr)(int irq, void *isr_data);
+	void *isr_data;
+
+	struct class *vd_class; /* for udev */
+};
+
+int wmt_vd_register(struct videodecoder *);
+int wmt_vd_unregister(struct videodecoder *);
+
+#endif
+