static int vibrator_pwm_set(int enable, int amp, int n_value)
{
uint M_VAL = GP_CLK_M_DEFAULT;
uint D_VAL = 0;
uint D_INV = 0;
uint clk_id = gp_clk_id;
pr_debug("%s: amp=%d, n_value=%d\n", __func__, amp, n_value);
if (enable) {
if (amp)
D_VAL = vibrator_adjust_amp(amp) + GP_CLK_D_HALF;
if (D_VAL > GP_CLK_D_HALF) {
D_VAL = GP_CLK_D_MAX - D_VAL;
D_INV = 1;
}
REG_WRITEL(
(((M_VAL & 0xffU) << 16U) + /* M_VAL[23:16] */
((~(D_VAL << 1)) & 0xffU)), /* D_VAL[7:0] */
GPn_MD_REG(clk_id));
REG_WRITEL(
((((~(n_value-M_VAL)) & 0xffU) << 16U) + /* N_VAL[23:16] */
(1U << 11U) + /* CLK_ROOT_ENA[11] : Enable(1) */
((D_INV & 0x01U) << 10U) + /* CLK_INV[10] : Disable(0) */
(1U << 9U) + /* CLK_BRANCH_ENA[9] : Enable(1) */
(1U << 8U) + /* NMCNTR_EN[8] : Enable(1) */
(0U << 7U) + /* MNCNTR_RST[7] : Not Active(0) */
(2U << 5U) + /* MNCNTR_MODE[6:5] : Dual-edge mode(2) */
(3U << 3U) + /* PRE_DIV_SEL[4:3] : Div-4 (3) */
(5U << 0U)), /* SRC_SEL[2:0] : CXO (5) */
GPn_NS_REG(clk_id));
pr_debug("%s: PWM is enable with M=%d N=%d D=%d\n",
__func__,
M_VAL, n_value, D_VAL);
} else {
REG_WRITEL(
((((~(n_value-M_VAL)) & 0xffU) << 16U) + /* N_VAL[23:16] */
(0U << 11U) + /* CLK_ROOT_ENA[11] : Disable(0) */
(0U << 10U) + /* CLK_INV[10] : Disable(0) */
(0U << 9U) + /* CLK_BRANCH_ENA[9] : Disable(0) */
(0U << 8U) + /* NMCNTR_EN[8] : Disable(0) */
(0U << 7U) + /* MNCNTR_RST[7] : Not Active(0) */
(2U << 5U) + /* MNCNTR_MODE[6:5] : Dual-edge mode(2) */
(3U << 3U) + /* PRE_DIV_SEL[4:3] : Div-4 (3) */
(5U << 0U)), /* SRC_SEL[2:0] : CXO (5) */
GPn_NS_REG(clk_id));
pr_debug("%s: PWM is disable\n", __func__);
}
return 0;
}
static int irrc_pwm_set(int enable,int PWM_CLK, int duty)
{
int M_VAL=1;
int N_VAL=1;
int D_VAL=1;
//pwm_clk khz = 19.2 Mhz * M_VAL / Predive *N_VAL
//N_VAL = floor(19200*M_VAL/PWM_CLK*4);
N_VAL = (9600+PWM_CLK)/(PWM_CLK*2);
D_VAL = (N_VAL*duty+50)/100;
if (D_VAL==0)
D_VAL=1;
printk(KERN_INFO "enable = %d ,M_VAL = %d, N_VAL = %d , D_VAL = %d\n",enable,M_VAL,N_VAL,D_VAL);
msm_xo_mode_vote(irrc_clock, MSM_XO_MODE_ON);
//PWM frequency setting
REG_WRITEL(
(((M_VAL & 0xffU) << 16U) + /* M_VAL[23:16] */
((~(D_VAL << 1)) & 0xffU)), /* D_VAL[7:0] */
GPn_MD_REG(GP_CLK_ID));
/* TODO: set clk for amp */
if (enable) {
////GP_CLK ON
////GP_CLK source 32khz select , predive Div -1 select
REG_WRITEL(
((((~(N_VAL-M_VAL)) & 0xffU) << 16U) + /* N_VAL[23:16] */
(0U << 11U) + /* CLK_ROOT_ENA[11] : Disable(0) */
(0U << 10U) + /* CLK_INV[10] : Disable(0) */
(0U << 9U) + /* CLK_BRANCH_ENA[9] : Disable(0) */
(0U << 8U) + /* NMCNTR_EN[8] : Disable(0) */
(1U << 7U) + /* MNCNTR_RST[7] : Not Active(0) */
(2U << 5U) + /* MNCNTR_MODE[6:5] : Dual-edge mode(2) */
(3U << 3U) + /* PRE_DIV_SEL[4:3] : Div-4(3) */
(5U << 0U)), /* SRC_SEL[2:0] : CXO (5) */
GPn_NS_REG(GP_CLK_ID));
REG_WRITEL(
((((~(N_VAL-M_VAL)) & 0xffU) << 16U) + /* N_VAL[23:16] */
(1U << 11U) + /* CLK_ROOT_ENA[11] : Enable(1) */
(0U << 10U) + /* CLK_INV[10] : Disable(0) */
(1U << 9U) + /* CLK_BRANCH_ENA[9] : Enable(1) */
(1U << 8U) + /* NMCNTR_EN[8] : Enable(1) */
(0U << 7U) + /* MNCNTR_RST[7] : Not Active(0) */
(2U << 5U) + /* MNCNTR_MODE[6:5] : Dual-edge mode(2) */
(3U << 3U) + /* PRE_DIV_SEL[4:3] : Div-4(3) */
(5U << 0U)), /* SRC_SEL[2:0] : CXO (5) */
GPn_NS_REG(GP_CLK_ID));
} else {
////GP_CLK OFF
REG_WRITEL(
((((~(N_VAL-M_VAL)) & 0xffU) << 16U) + /* N_VAL[23:16] */
(0U << 11U) + /* CLK_ROOT_ENA[11] : Disable(0) */
(0U << 10U) + /* CLK_INV[10] : Disable(0) */
(0U << 9U) + /* CLK_BRANCH_ENA[9] : Disable(0) */
(0U << 8U) + /* NMCNTR_EN[8] : Disable(0) */
(1U << 7U) + /* MNCNTR_RST[7] : Not Active(0) */
(2U << 5U) + /* MNCNTR_MODE[6:5] : Dual-edge mode(2) */
(3U << 3U) + /* PRE_DIV_SEL[4:3] : Div-4 (3) */
(5U << 0U)), /* SRC_SEL[2:0] : CXO (5) */
GPn_NS_REG(GP_CLK_ID));
}
return 0;
}
