From eb5123d5366b5bd894da667d550c7820f9dc4a92 Mon Sep 17 00:00:00 2001 From: Konstantin Makariev Date: Tue, 28 Jun 2022 23:47:22 -0500 Subject: [PATCH] drv8424_mmi: unify source code indentation Replace leading white spaces with tabs Change-Id: I6c26f0083cb5bae6a6c76a6fdb7e2a35b28ea936 Signed-off-by: Konstantin Makariev Reviewed-on: https://gerrit.mot.com/2316810 SME-Granted: SME Approvals Granted SLTApproved: Slta Waiver Tested-by: Jira Key Reviewed-by: Qing Chang Submit-Approved: Jira Key --- drivers/misc/drv8424/drv8424.c | 2108 +++++++++++++++----------------- 1 file changed, 1004 insertions(+), 1104 deletions(-) diff --git a/drivers/misc/drv8424/drv8424.c b/drivers/misc/drv8424/drv8424.c index d9be8de7124c..0676133d7874 100644 --- a/drivers/misc/drv8424/drv8424.c +++ b/drivers/misc/drv8424/drv8424.c @@ -67,8 +67,8 @@ enum position_id { }; static const char *position_labels[] = { - "UNKNOWN", - "COMPACT", + "UNKNOWN", + "COMPACT", "EXPANDED", "PEEK", }; @@ -106,9 +106,9 @@ enum status_id { }; static const char *status_labels[] = { - "UNKNOWN", - "STOPPED_COMPACT", - "STOPPED_EXPANDED", + "UNKNOWN", + "STOPPED_COMPACT", + "STOPPED_EXPANDED", "STOPPED_PEEK", "EXPANDING", "WITHDRAWING", @@ -117,105 +117,105 @@ static const char *status_labels[] = { }; enum gpios_index { - MOTOR_POWER_EN = 0, /* BOOST */ - MOTOR_FAULT1_INT, /* nFAULT1 INT */ - MOTOR_FAULT2_INT, /* nFAULT2 INT */ - MOTOR_STEP, /* STEP */ - MOTOR_DIR, /* DIR */ - MOTOR_MODE1, /* set to 0 in HW */ - MOTOR_MODE2, /* set to 0 in HW */ - MOTOR_EN, /* set to 1 in HW */ - MOTOR_SLEEP, /* nSLEEP */ - MOTOR_T0, /* set to 0 in HW */ - MOTOR_T1, /* set to 0 in HW */ + MOTOR_POWER_EN = 0, /* BOOST */ + MOTOR_FAULT1_INT, /* nFAULT1 INT */ + MOTOR_FAULT2_INT, /* nFAULT2 INT */ + MOTOR_STEP, /* STEP */ + MOTOR_DIR, /* DIR */ + MOTOR_MODE1, /* set to 0 in HW */ + MOTOR_MODE2, /* set to 0 in HW */ + MOTOR_EN, /* set to 1 in HW */ + MOTOR_SLEEP, /* nSLEEP */ + MOTOR_T0, /* set to 0 in HW */ + MOTOR_T1, /* set to 0 in HW */ MOTOR_ACTIVE, /* signal GPIO */ - MOTOR_MAX_GPIO, - MOTOR_UNKNOWN = MOTOR_MAX_GPIO + MOTOR_MAX_GPIO, + MOTOR_UNKNOWN = MOTOR_MAX_GPIO }; static const char* const gpios_labels[] = { - "MOTOR_POWER_EN", - "MOTOR_FAULT1_INT", - "MOTOR_FAULT2_INT", - "MOTOR_STEP", - "MOTOR_DIR", - "MOTOR_MODE1", - "MOTOR_MODE2", - "MOTOR_EN", - "MOTOR_SLEEP", - "MOTOR_T0", - "MOTOR_T1", + "MOTOR_POWER_EN", + "MOTOR_FAULT1_INT", + "MOTOR_FAULT2_INT", + "MOTOR_STEP", + "MOTOR_DIR", + "MOTOR_MODE1", + "MOTOR_MODE2", + "MOTOR_EN", + "MOTOR_SLEEP", + "MOTOR_T0", + "MOTOR_T1", "MOTOR_ACTIVE", - "MOTOR_UNKNOWN" + "MOTOR_UNKNOWN" }; static const unsigned long hw_clocks[] = { - DEFAULT_STEP_FREQ, //0,0 software clock - 4800, //0,Z 2*2400 - 9600, //0,1 4*2400 - 19200, //Z,0 8*2400 - 9600, //Z,Z 4*2400 - 38400, //Z,1 16*2400 - 19200, //1,0 8*2400 - 76800, //1,Z 32*2400 - 38400, //1,1 16*2400 + DEFAULT_STEP_FREQ, //0,0 software clock + 4800, //0,Z 2*2400 + 9600, //0,1 4*2400 + 19200, //Z,0 8*2400 + 9600, //Z,Z 4*2400 + 38400, //Z,1 16*2400 + 19200, //1,0 8*2400 + 76800, //1,Z 32*2400 + 38400, //1,1 16*2400 }; enum motor_step_mode { - FULL_STEP = 0, //0,0 - STEP_2_RISING, //0,Z - STEP_4_RISING, //0,1 - STEP_8_RISING, //Z,0 - STEP_8_BOTH_EDEG, //Z,Z - STEP_16_RISING, //Z,1 - STEP_16_BOTH_EDGE, //1,0 - STEP_32_RISING, //1,Z - STEP_32_BOTH_EDEG, //1,1 + FULL_STEP = 0, //0,0 + STEP_2_RISING, //0,Z + STEP_4_RISING, //0,1 + STEP_8_RISING, //Z,0 + STEP_8_BOTH_EDEG, //Z,Z + STEP_16_RISING, //Z,1 + STEP_16_BOTH_EDGE, //1,0 + STEP_32_RISING, //1,Z + STEP_32_BOTH_EDEG, //1,1 }; enum motor_torque { - TORQUE_FULL = 0, //0,0 - TORQUE_87, //0,Z - TORQUE_75, //0,1 - TORQUE_62, //Z,0 - TORQUE_50, //Z,Z - TORQUE_37, //Z,1 - TORQUE_25, //1,0 - TORQUE_12, //1,Z - TORQUE_DISABLE, //1,1 + TORQUE_FULL = 0, //0,0 + TORQUE_87, //0,Z + TORQUE_75, //0,1 + TORQUE_62, //Z,0 + TORQUE_50, //Z,Z + TORQUE_37, //Z,1 + TORQUE_25, //1,0 + TORQUE_12, //1,Z + TORQUE_DISABLE, //1,1 }; //Keep in sequence with dtsi. enum motor_pins_mode { - MODE0_LOW = 0, - MODE0_HIGH, - MODE0_DISABLE, - MODE1_LOW, - MODE1_HIGH, - MODE1_DISABLE, - T0_LOW, - T0_HIGH, - T0_DISABLE, - T1_LOW, - T1_HIGH, - T1_DISABLE, - INT_DEFAULT, - CLK_ACTIVE, - CLK_SLEEP, - INIT_STATE, - PINS_END + MODE0_LOW = 0, + MODE0_HIGH, + MODE0_DISABLE, + MODE1_LOW, + MODE1_HIGH, + MODE1_DISABLE, + T0_LOW, + T0_HIGH, + T0_DISABLE, + T1_LOW, + T1_HIGH, + T1_DISABLE, + INT_DEFAULT, + CLK_ACTIVE, + CLK_SLEEP, + INIT_STATE, + PINS_END }; //Keep in same name with the pinctrl-names of dtsi static const char* const pins_state[] = { - "m0_low", "m0_high", "m0_disable", - "m1_low", "m1_high", "m1_disable", - "t0_low", "t0_high", "t0_disable", - "t1_low", "t1_high", "t1_disable", - "drv8424_int_default", - "drv8424_clk_active", "drv8424_clk_sleep", - "init_state", - NULL + "m0_low", "m0_high", "m0_disable", + "m1_low", "m1_high", "m1_disable", + "t0_low", "t0_high", "t0_disable", + "t1_low", "t1_high", "t1_disable", + "drv8424_int_default", + "drv8424_clk_active", "drv8424_clk_sleep", + "init_state", + NULL }; enum regime_idx { @@ -282,68 +282,68 @@ enum cmds { #define MAX_GPIOS MOTOR_UNKNOWN typedef struct motor_control { - struct regulator *vdd; - int32_t ptable[MOTOR_MAX_GPIO]; - size_t tab_cells; - struct pinctrl* pins; - struct pinctrl_state *pin_state[PINS_END]; - const char* const * plabels; + struct regulator *vdd; + int32_t ptable[MOTOR_MAX_GPIO]; + size_t tab_cells; + struct pinctrl* pins; + struct pinctrl_state *pin_state[PINS_END]; + const char* const * plabels; } motor_control; #define CLOCK_NAME_LEN 16 typedef struct motor_device { - char clock_name[CLOCK_NAME_LEN]; - struct device* dev; - struct device* sysfs_dev; - struct class* drv8424_class; - struct workqueue_struct* motor_wq; - struct clk * pwm_clk; - struct delayed_work motor_work; - struct task_struct *motor_task; - wait_queue_head_t sync_complete; - wait_queue_head_t status_wait; - wait_queue_head_t position_wait; - struct hrtimer stepping_timer; - struct hrtimer timeout_timer; - motor_control mc; - spinlock_t mlock; - struct mutex mx_lock; - atomic_t fault_irq; - bool faulting; - unsigned step_freq; - unsigned long step_period; - unsigned long step_ceiling; - unsigned long cur_clk; - atomic_t step_count; - atomic_t stepping; - atomic_t status; - atomic_t position; - atomic_t destination; - unsigned slot; - unsigned mode, cur_mode; - unsigned torque; - unsigned time_out; - unsigned half; + char clock_name[CLOCK_NAME_LEN]; + struct device* dev; + struct device* sysfs_dev; + struct class* drv8424_class; + struct workqueue_struct* motor_wq; + struct clk * pwm_clk; + struct delayed_work motor_work; + struct task_struct *motor_task; + wait_queue_head_t sync_complete; + wait_queue_head_t status_wait; + wait_queue_head_t position_wait; + struct hrtimer stepping_timer; + struct hrtimer timeout_timer; + motor_control mc; + spinlock_t mlock; + struct mutex mx_lock; + atomic_t fault_irq; + bool faulting; + unsigned step_freq; + unsigned long step_period; + unsigned long step_ceiling; + unsigned long cur_clk; + atomic_t step_count; + atomic_t stepping; + atomic_t status; + atomic_t position; + atomic_t destination; + unsigned slot; + unsigned mode, cur_mode; + unsigned torque; + unsigned time_out; + unsigned half; unsigned stage, max_stages; unsigned regime; motor_stage sequencer[SQ_MAX][MAX_STAGE_LEGS]; - bool double_edge; - bool hw_clock; - bool power_default_off; - bool support_mode; - bool support_torque; - bool sensors_off; - bool irq_enabled; - int level:1; - unsigned power_en:1; - unsigned nsleep:1; - unsigned nEN:1; - unsigned dir:1; - unsigned user_sync_complete:1; + bool double_edge; + bool hw_clock; + bool power_default_off; + bool support_mode; + bool support_torque; + bool sensors_off; + bool irq_enabled; + int level:1; + unsigned power_en:1; + unsigned nsleep:1; + unsigned nEN:1; + unsigned dir:1; + unsigned user_sync_complete:1; unsigned status_update_ready:1; unsigned position_update_ready:1; struct kfifo cmd_pipe; - int sensor_data[3]; + int sensor_data[3]; } motor_device; #define RESET_SENSOR_DATA do { \ @@ -380,183 +380,174 @@ static void moto_drv8424_set_step_freq(motor_device* md, unsigned freq); static ktime_t adapt_time_helper(ktime_t usec) { - return ns_to_ktime(usec * 1000); + return ns_to_ktime(usec * 1000); } #if 0 static inline bool is_hw_clk(motor_device* md) { - return (md->hw_clock && (md->cur_clk != hw_clocks[0])); + return (md->hw_clock && (md->cur_clk != hw_clocks[0])); } #endif void sleep_helper(unsigned usec) { - if(usec < 500) { //<500us, udelay - udelay(usec); - } else if(usec >= 500 && usec < 20000) { //500us - 20ms, usleep_range - usleep_range(usec, usec + 10); - } else { //>= 20ms, msleep - msleep(usec); - } + if (usec < 500) { //<500us, udelay + udelay(usec); + } else if (usec >= 500 && usec < 20000) { //500us - 20ms, usleep_range + usleep_range(usec, usec + 10); + } else { //>= 20ms, msleep + msleep(usec); + } } static int moto_drv8424_set_regulator_power(motor_device* md, bool en) { - motor_control * mc = &md->mc; - int err = 0; - - if(en) { - err = regulator_enable(mc->vdd); - if (err) { - dev_err(md->dev, "Failed to enable VDD ret=%d\n", err); - goto exit; - } - LOGD("regulator enabled\n"); - } else { - err = regulator_disable(mc->vdd); - if (err) { - dev_err(md->dev, "Failed to disable VDD ret=%d\n", err); - goto exit; - } - LOGD("regulator disabled\n"); - } - - return 0; + motor_control * mc = &md->mc; + int err = 0; + if (en) { + err = regulator_enable(mc->vdd); + if (err) { + dev_err(md->dev, "Failed to enable VDD ret=%d\n", err); + goto exit; + } + LOGD("regulator enabled\n"); + } else { + err = regulator_disable(mc->vdd); + if (err) { + dev_err(md->dev, "Failed to disable VDD ret=%d\n", err); + goto exit; + } + LOGD("regulator disabled\n"); + } + return 0; exit: - return err; + return err; } #if 0 static int set_motor_clk(motor_device* md, bool en) { - int ret = 0; - - if(en) { - set_pinctrl_state(md, CLK_ACTIVE); - ret = clk_set_rate(md->pwm_clk, md->cur_clk); - if(ret < 0) { - dev_err(md->dev, "Failed to set clk rate to %ld\n", md->cur_clk); - ret = -ENODEV; - goto soft_clk; - } - ret = clk_prepare_enable(md->pwm_clk); - if(ret < 0) { - dev_err(md->dev, "Failed to clk_prepare_enable\n"); - ret = -ENODEV; - goto soft_clk; - } - } else { - clk_disable_unprepare(md->pwm_clk); - dev_info(md->dev, "disable clock"); - set_pinctrl_state(md, CLK_SLEEP); - } - - return 0; + int ret = 0; + if (en) { + set_pinctrl_state(md, CLK_ACTIVE); + ret = clk_set_rate(md->pwm_clk, md->cur_clk); + if (ret < 0) { + dev_err(md->dev, "Failed to set clk rate to %ld\n", md->cur_clk); + ret = -ENODEV; + goto soft_clk; + } + ret = clk_prepare_enable(md->pwm_clk); + if (ret < 0) { + dev_err(md->dev, "Failed to clk_prepare_enable\n"); + ret = -ENODEV; + goto soft_clk; + } + } else { + clk_disable_unprepare(md->pwm_clk); + dev_info(md->dev, "disable clock"); + set_pinctrl_state(md, CLK_SLEEP); + } + return 0; soft_clk: - md->hw_clock = false; - return ret; + md->hw_clock = false; + return ret; } #endif static int init_motor_clk(motor_device* md) { - int ret = 0; + int ret = 0; - md->pwm_clk = devm_clk_get(md->dev, md->clock_name); - if(IS_ERR(md->pwm_clk)) { - dev_err(md->dev, "Get clk error, motor is not drived\n"); - ret = -ENODEV; - goto soft_clk; - } - - return 0; + md->pwm_clk = devm_clk_get(md->dev, md->clock_name); + if (IS_ERR(md->pwm_clk)) { + dev_err(md->dev, "Get clk error, motor is not drived\n"); + ret = -ENODEV; + goto soft_clk; + } + return 0; soft_clk: - md->hw_clock = false; - return ret; + md->hw_clock = false; + return ret; } static int set_pinctrl_state(motor_device* md, unsigned state_index) { - motor_control* mc = &md->mc; - int ret = 0; + motor_control* mc = &md->mc; + int ret = 0; - if(state_index >= PINS_END) { - dev_err(md->dev, "Illegal pin index\n"); - ret = -EINVAL; - goto err; - } - - dev_dbg(md->dev, "setting punctrl for state '%s'\n", pins_state[state_index]); - mc->pins = devm_pinctrl_get(md->dev); - if(IS_ERR_OR_NULL(mc->pins)) { - ret = PTR_ERR(mc->pins); - dev_err(md->dev, "Failed to get pinctrl %d\n", ret); - goto err; - } - - mc->pin_state[state_index] = pinctrl_lookup_state(mc->pins, pins_state[state_index]); - if (IS_ERR_OR_NULL(mc->pin_state[state_index])) { - ret = PTR_ERR(mc->pin_state[state_index]); - dev_err(md->dev, "Cannot find pin_state '%s' %d\n", pins_state[state_index], ret); - goto err_pin_state; - } - - ret = pinctrl_select_state(mc->pins, mc->pin_state[state_index]); - if(ret) { - dev_err(md->dev, "Cannot set pin_state '%s' %d\n", pins_state[state_index], ret); - } + if (state_index >= PINS_END) { + dev_err(md->dev, "Illegal pin index\n"); + ret = -EINVAL; + goto err; + } + dev_dbg(md->dev, "setting punctrl for state '%s'\n", pins_state[state_index]); + mc->pins = devm_pinctrl_get(md->dev); + if (IS_ERR_OR_NULL(mc->pins)) { + ret = PTR_ERR(mc->pins); + dev_err(md->dev, "Failed to get pinctrl %d\n", ret); + goto err; + } + mc->pin_state[state_index] = pinctrl_lookup_state(mc->pins, pins_state[state_index]); + if (IS_ERR_OR_NULL(mc->pin_state[state_index])) { + ret = PTR_ERR(mc->pin_state[state_index]); + dev_err(md->dev, "Cannot find pin_state '%s' %d\n", pins_state[state_index], ret); + goto err_pin_state; + } + ret = pinctrl_select_state(mc->pins, mc->pin_state[state_index]); + if (ret) { + dev_err(md->dev, "Cannot set pin_state '%s' %d\n", pins_state[state_index], ret); + } err_pin_state: - pinctrl_put(mc->pins); + pinctrl_put(mc->pins); err: - return ret; + return ret; } static void moto_drv8424_set_motor_torque(motor_device* md) { if (!md->support_torque) return; - - switch(md->torque) { - case TORQUE_FULL: - set_pinctrl_state(md, T0_LOW); - set_pinctrl_state(md, T1_LOW); - break; - case TORQUE_87: - set_pinctrl_state(md, T0_DISABLE); - set_pinctrl_state(md, T1_LOW); - break; - case TORQUE_75: - set_pinctrl_state(md, T0_HIGH); - set_pinctrl_state(md, T1_LOW); - break; - case TORQUE_62: - set_pinctrl_state(md, T0_LOW); - set_pinctrl_state(md, T1_DISABLE); - break; - case TORQUE_50: - set_pinctrl_state(md, T0_DISABLE); - set_pinctrl_state(md, T1_DISABLE); - break; - case TORQUE_37: - set_pinctrl_state(md, T0_HIGH); - set_pinctrl_state(md, T1_DISABLE); - break; - case TORQUE_25: - set_pinctrl_state(md, T0_LOW); - set_pinctrl_state(md, T1_HIGH); - break; - case TORQUE_12: - set_pinctrl_state(md, T0_DISABLE); - set_pinctrl_state(md, T1_HIGH); - break; - case TORQUE_DISABLE: - set_pinctrl_state(md, T0_HIGH); - set_pinctrl_state(md, T1_HIGH); - break; - default: - pr_err("Unsupported torque %d\n", md->torque); - return; - } + switch (md->torque) { + case TORQUE_FULL: + set_pinctrl_state(md, T0_LOW); + set_pinctrl_state(md, T1_LOW); + break; + case TORQUE_87: + set_pinctrl_state(md, T0_DISABLE); + set_pinctrl_state(md, T1_LOW); + break; + case TORQUE_75: + set_pinctrl_state(md, T0_HIGH); + set_pinctrl_state(md, T1_LOW); + break; + case TORQUE_62: + set_pinctrl_state(md, T0_LOW); + set_pinctrl_state(md, T1_DISABLE); + break; + case TORQUE_50: + set_pinctrl_state(md, T0_DISABLE); + set_pinctrl_state(md, T1_DISABLE); + break; + case TORQUE_37: + set_pinctrl_state(md, T0_HIGH); + set_pinctrl_state(md, T1_DISABLE); + break; + case TORQUE_25: + set_pinctrl_state(md, T0_LOW); + set_pinctrl_state(md, T1_HIGH); + break; + case TORQUE_12: + set_pinctrl_state(md, T0_DISABLE); + set_pinctrl_state(md, T1_HIGH); + break; + case TORQUE_DISABLE: + set_pinctrl_state(md, T0_HIGH); + set_pinctrl_state(md, T1_HIGH); + break; + default: + pr_err("Unsupported torque %d\n", md->torque); + return; + } LOGD("pins T0 & T1 updated\n"); } @@ -566,73 +557,71 @@ static void moto_drv8424_set_motor_mode(motor_device* md) md->cur_mode = md->mode; return; } - - switch(md->mode) { - case FULL_STEP: - set_pinctrl_state(md, MODE0_LOW); - set_pinctrl_state(md, MODE1_LOW); - break; - case STEP_2_RISING: - set_pinctrl_state(md, MODE0_DISABLE); - set_pinctrl_state(md, MODE1_LOW); - break; - case STEP_4_RISING: - set_pinctrl_state(md, MODE0_HIGH); - set_pinctrl_state(md, MODE1_LOW); - break; - case STEP_8_RISING: - set_pinctrl_state(md, MODE0_LOW); - set_pinctrl_state(md, MODE1_DISABLE); - break; - case STEP_8_BOTH_EDEG: - set_pinctrl_state(md, MODE0_DISABLE); - set_pinctrl_state(md, MODE1_DISABLE); - break; - case STEP_16_RISING: - set_pinctrl_state(md, MODE0_HIGH); - set_pinctrl_state(md, MODE1_DISABLE); - break; - case STEP_16_BOTH_EDGE: - set_pinctrl_state(md, MODE0_LOW); - set_pinctrl_state(md, MODE1_HIGH); - break; - case STEP_32_RISING: - set_pinctrl_state(md, MODE0_DISABLE); - set_pinctrl_state(md, MODE1_HIGH); - break; - case STEP_32_BOTH_EDEG: - set_pinctrl_state(md, MODE0_HIGH); - set_pinctrl_state(md, MODE1_HIGH); - break; - default: - pr_err("Unsupported mode %d\n", md->mode); - return; - } - - if(md->cur_mode != md->mode) { - if(md->hw_clock) { - if(md->mode == FULL_STEP) { - md->cur_clk = hw_clocks[md->mode]; - set_pinctrl_state(md, CLK_SLEEP); - dev_info(md->dev, "md->mode is FULL_STEP\n"); - } else { - md->cur_clk = hw_clocks[md->mode]; - dev_info(md->dev, "Switch hw clock\n"); - } - } + switch (md->mode) { + case FULL_STEP: + set_pinctrl_state(md, MODE0_LOW); + set_pinctrl_state(md, MODE1_LOW); + break; + case STEP_2_RISING: + set_pinctrl_state(md, MODE0_DISABLE); + set_pinctrl_state(md, MODE1_LOW); + break; + case STEP_4_RISING: + set_pinctrl_state(md, MODE0_HIGH); + set_pinctrl_state(md, MODE1_LOW); + break; + case STEP_8_RISING: + set_pinctrl_state(md, MODE0_LOW); + set_pinctrl_state(md, MODE1_DISABLE); + break; + case STEP_8_BOTH_EDEG: + set_pinctrl_state(md, MODE0_DISABLE); + set_pinctrl_state(md, MODE1_DISABLE); + break; + case STEP_16_RISING: + set_pinctrl_state(md, MODE0_HIGH); + set_pinctrl_state(md, MODE1_DISABLE); + break; + case STEP_16_BOTH_EDGE: + set_pinctrl_state(md, MODE0_LOW); + set_pinctrl_state(md, MODE1_HIGH); + break; + case STEP_32_RISING: + set_pinctrl_state(md, MODE0_DISABLE); + set_pinctrl_state(md, MODE1_HIGH); + break; + case STEP_32_BOTH_EDEG: + set_pinctrl_state(md, MODE0_HIGH); + set_pinctrl_state(md, MODE1_HIGH); + break; + default: + pr_err("Unsupported mode %d\n", md->mode); + return; + } + if (md->cur_mode != md->mode) { + if (md->hw_clock) { + if (md->mode == FULL_STEP) { + md->cur_clk = hw_clocks[md->mode]; + set_pinctrl_state(md, CLK_SLEEP); + dev_info(md->dev, "md->mode is FULL_STEP\n"); + } else { + md->cur_clk = hw_clocks[md->mode]; + dev_info(md->dev, "Switch hw clock\n"); + } + } md->cur_mode = md->mode; - md->cur_mode = md->mode; - } - usleep_range(800, 900); + md->cur_mode = md->mode; + } + usleep_range(800, 900); LOGD("pins MODE0 & MODE1 updated\n"); } static void moto_drv8424_set_motor_dir(motor_device* md) { - motor_control* mc = &md->mc; + motor_control* mc = &md->mc; - GPIO_OUTPUT_DIR(mc->ptable[MOTOR_DIR], md->dir); - usleep_range(800, 900); + GPIO_OUTPUT_DIR(mc->ptable[MOTOR_DIR], md->dir); + usleep_range(800, 900); LOGD("pin DIR set %d\n", md->dir); } @@ -645,42 +634,39 @@ static void moto_drv8424_set_motor_dir(motor_device* md) */ static void moto_drv8424_set_motor_opmode(motor_device* md) { - motor_control* mc = &md->mc; + motor_control* mc = &md->mc; - GPIO_OUTPUT_DIR(mc->ptable[MOTOR_SLEEP], md->nsleep); - //Twake 0.5ms Tsleep 0.7ms - usleep_range(800, 1000); + GPIO_OUTPUT_DIR(mc->ptable[MOTOR_SLEEP], md->nsleep); + //Twake 0.5ms Tsleep 0.7ms + usleep_range(800, 1000); GPIO_OUTPUT_DIR(mc->ptable[MOTOR_EN], md->nEN); LOGD("pin SLEEP set %d\n", md->nsleep); } static int moto_drv8424_set_opmode(motor_device* md, unsigned opmode) { - unsigned long flags; - int ret = 0; + unsigned long flags; + int ret = 0; - spin_lock_irqsave(&md->mlock, flags); - - if(md->nsleep == opmode) { - dev_info(md->dev, "Unchanged the opmode status, ignore\n"); - ret = -EINVAL; - goto exit; - } - md->nsleep = !!opmode; - md->nEN = !md->nsleep; - spin_unlock_irqrestore(&md->mlock, flags); + spin_lock_irqsave(&md->mlock, flags); + if (md->nsleep == opmode) { + dev_info(md->dev, "Unchanged the opmode status, ignore\n"); + ret = -EINVAL; + goto exit; + } + md->nsleep = !!opmode; + md->nEN = !md->nsleep; + spin_unlock_irqrestore(&md->mlock, flags); LOGD("nsleep=%d, en=%d\n", md->nsleep, md->nEN); - - return 0; - + return 0; exit: - spin_unlock_irqrestore(&md->mlock, flags); - return ret; + spin_unlock_irqrestore(&md->mlock, flags); + return ret; } static void inline moto_drv8424_cmd_push(motor_device* md, - int cmd, unsigned long delay) + int cmd, unsigned long delay) { kfifo_put(&md->cmd_pipe, cmd); queue_delayed_work(md->motor_wq, &md->motor_work, delay); @@ -688,11 +674,10 @@ static void inline moto_drv8424_cmd_push(motor_device* md, static void moto_drv8424_set_sensing(motor_device* md, bool en) { - motor_control* mc = &md->mc; + motor_control* mc = &md->mc; if (md->sensors_off) return; - if (en) { /* signal sensor hub to start/stop sampling hall effect */ GPIO_OUTPUT_DIR(mc->ptable[MOTOR_ACTIVE], 1); @@ -714,11 +699,10 @@ static void moto_drv8424_set_sensing(motor_device* md, bool en) //Set VDD static void moto_drv8424_set_power_en(motor_device* md) { - motor_control* mc = &md->mc; - - GPIO_OUTPUT_DIR(mc->ptable[MOTOR_POWER_EN], md->power_en); - //Tpower 0.5ms - usleep_range(500, 1000); + motor_control* mc = &md->mc; + GPIO_OUTPUT_DIR(mc->ptable[MOTOR_POWER_EN], md->power_en); + //Tpower 0.5ms + usleep_range(500, 1000); LOGD("BOOST set %d\n", md->power_en); moto_drv8424_set_sensing(md, md->power_en ? true : false); } @@ -726,53 +710,49 @@ static void moto_drv8424_set_power_en(motor_device* md) //Power sequence: PowerEn On->nSleep On->nSleep Off->PowerEn Off static int moto_drv8424_set_power(motor_device* md, unsigned power) { - unsigned long flags; - int ret = 0; + unsigned long flags; + int ret = 0; - if(md->power_en == power) { - dev_info(md->dev, "Unchanged the power status, ignore\n"); - ret = -EINVAL; - goto exit; - } + if (md->power_en == power) { + dev_info(md->dev, "Unchanged the power status, ignore\n"); + ret = -EINVAL; + goto exit; + } LOGD("power_en=%u\n", !!power); - spin_lock_irqsave(&md->mlock, flags); - md->power_en = !!power; - spin_unlock_irqrestore(&md->mlock, flags); - - ret = moto_drv8424_set_opmode(md, md->power_en); - if(ret < 0 ) { - goto exit; - } - if(!md->power_en) { - moto_drv8424_set_motor_opmode(md); - } - moto_drv8424_set_power_en(md); - - return 0; - + spin_lock_irqsave(&md->mlock, flags); + md->power_en = !!power; + spin_unlock_irqrestore(&md->mlock, flags); + ret = moto_drv8424_set_opmode(md, md->power_en); + if (ret < 0 ) { + goto exit; + } + if (!md->power_en) { + moto_drv8424_set_motor_opmode(md); + } + moto_drv8424_set_power_en(md); + return 0; exit: - return ret; + return ret; } static void moto_drv8424_set_regime(motor_device *md, unsigned regime) { unsigned msn; - unsigned long flags; + unsigned long flags; for (msn = 0; msn < MAX_STAGE_LEGS; msn++) { if (md->sequencer[regime][msn].freq && md->sequencer[regime][msn].ceiling) continue; break; } - - spin_lock_irqsave(&md->mlock, flags); + spin_lock_irqsave(&md->mlock, flags); md->regime = regime; md->stage = 0; - md->max_stages = msn; + md->max_stages = msn; md->time_out = timeoutMs[regime]; - spin_unlock_irqrestore(&md->mlock, flags); - /* TODO: make time out based on sequence duration? */ - md->time_out = MOTOR_DEFAULT_EXPIRE; + spin_unlock_irqrestore(&md->mlock, flags); + /* TODO: make time out based on sequence duration? */ + md->time_out = MOTOR_DEFAULT_EXPIRE; dev_info(md->dev, "Set active regime: %s, stages # %u\n", regime_names[md->regime], msn); @@ -781,7 +761,7 @@ static void moto_drv8424_set_regime(motor_device *md, unsigned regime) #if 0 static int moto_drv8424_enable_clk(motor_device* md, bool en) { - return set_motor_clk(md, en); + return set_motor_clk(md, en); } #endif #ifdef MOTOR_SLOT_DBG @@ -822,17 +802,16 @@ static void inline logtime_reset(void) static void moto_drv8424_drive_stage_init(motor_device* md) { logtime_reset(); - - atomic_set(&md->stepping, 1); - atomic_set(&md->step_count, 1); - md->double_edge = false; - md->half = md->step_period >> 1; - md->level = 1; - if(md->mode == STEP_8_BOTH_EDEG - || md->mode == STEP_16_BOTH_EDGE - || md->mode == STEP_32_BOTH_EDEG) { - md->double_edge = true; - } + atomic_set(&md->stepping, 1); + atomic_set(&md->step_count, 1); + md->double_edge = false; + md->half = md->step_period >> 1; + md->level = 1; + if (md->mode == STEP_8_BOTH_EDEG + || md->mode == STEP_16_BOTH_EDGE + || md->mode == STEP_32_BOTH_EDEG) { + md->double_edge = true; + } } static bool moto_drv8424_next_stage(motor_device* md) @@ -859,33 +838,32 @@ static bool moto_drv8424_next_stage(motor_device* md) static int moto_drv8424_drive_sequencer(motor_device* md) { - if (md->faulting) { - dev_warn(md->dev, "Motor faulting: action cancelled!!!\n"); - return -EBUSY; - } + if (md->faulting) { + dev_warn(md->dev, "Motor faulting: action cancelled!!!\n"); + return -EBUSY; + } moto_drv8424_next_stage(md); - moto_drv8424_set_motor_torque(md); - moto_drv8424_set_motor_dir(md); - moto_drv8424_set_motor_mode(md); - moto_drv8424_set_motor_opmode(md); + moto_drv8424_set_motor_torque(md); + moto_drv8424_set_motor_dir(md); + moto_drv8424_set_motor_mode(md); + moto_drv8424_set_motor_opmode(md); /* can judge about moving direction based on DIR or destination position */ atomic_set(&md->status, md->dir ? STATUS_MOVING_OUT : STATUS_MOVING_IN); moto_drv8424_cmd_push(md, CMD_STATUS, 0); LOGD("sequencer init: stages %u ceiling %lu, freq %uHz period %luns, half %uns\n", md->max_stages, md->step_ceiling, md->step_freq, md->step_period, md->half); - - if(atomic_read(&md->stepping)) { - motor_control* mc = &md->mc; + if (atomic_read(&md->stepping)) { + motor_control* mc = &md->mc; LOGD("Status updated: status %d\n", atomic_read(&md->status)); - GPIO_OUTPUT_DIR(mc->ptable[MOTOR_STEP], md->level); - atomic_inc(&md->step_count); - hrtimer_start(&md->stepping_timer, adapt_time_helper(md->half), HRTIMER_MODE_REL); + GPIO_OUTPUT_DIR(mc->ptable[MOTOR_STEP], md->level); + atomic_inc(&md->step_count); + hrtimer_start(&md->stepping_timer, adapt_time_helper(md->half), HRTIMER_MODE_REL); logtime_store(); - } + } - return 0; + return 0; } /* this function is called with spinlock running!!! */ @@ -893,129 +871,124 @@ static void inline motor_stop(motor_device* md, bool clean) { //LOGD("step count %d\n", atomic_read(&md->step_count)); if (clean) { - atomic_set(&md->step_count, 0); - atomic_set(&md->stepping, 0); + atomic_set(&md->step_count, 0); + atomic_set(&md->stepping, 0); //LOGD("step_count & stepping reset\n"); } //LOGD("waking up kthread\n"); - md->user_sync_complete = true; - wake_up(&md->sync_complete); + md->user_sync_complete = true; + wake_up(&md->sync_complete); } static __ref int motor_kthread(void *arg) { - motor_device* md = (motor_device*)arg; - struct sched_param param = {.sched_priority = MAX_USER_RT_PRIO - 1}; - int value, ret = 0; + motor_device* md = (motor_device*)arg; + struct sched_param param = {.sched_priority = MAX_USER_RT_PRIO - 1}; + int value, ret = 0; - sched_setscheduler(current, SCHED_FIFO, ¶m); - while (!kthread_should_stop()) { - LOGD("wait for event\n"); - do { - ret = wait_event_interruptible(md->sync_complete, - md->user_sync_complete || kthread_should_stop()); - } while (ret != 0); + sched_setscheduler(current, SCHED_FIFO, ¶m); + while (!kthread_should_stop()) { + LOGD("wait for event\n"); + do { + ret = wait_event_interruptible(md->sync_complete, + md->user_sync_complete || kthread_should_stop()); + } while (ret != 0); + if (kthread_should_stop()) + break; + md->user_sync_complete = false; + if (md->faulting) { + moto_drv8424_cmd_push(md, CMD_FAULT, 0); + atomic_set(&md->status, STATUS_FAULTING); + moto_drv8424_cmd_push(md, CMD_STATUS, 0); + dev_warn(md->dev, "Motor #%d failure reported!!!\n", + irq_to_gpio(atomic_read(&md->fault_irq)) == + md->mc.ptable[MOTOR_FAULT1_INT] ? 1 : 2);; + } else { + /* it's safe to call the following functions even if motor was not running */ + moto_drv8424_set_power(md, 0); + if (md->power_default_off) { + dev_info(md->dev, "vdd power off\n"); + moto_drv8424_set_regulator_power(md, false); + } - if(kthread_should_stop()) - break; + /* if position detected based on sensor data, + * current position will be set to destination + * to follow the same logic flow + */ + atomic_set(&md->position, atomic_read(&md->destination)); + moto_drv8424_cmd_push(md, CMD_POSITION, 0); - md->user_sync_complete = false; - if (md->faulting) { - moto_drv8424_cmd_push(md, CMD_FAULT, 0); - atomic_set(&md->status, STATUS_FAULTING); - moto_drv8424_cmd_push(md, CMD_STATUS, 0); - dev_warn(md->dev, "Motor #%d failure reported!!!\n", - irq_to_gpio(atomic_read(&md->fault_irq)) == - md->mc.ptable[MOTOR_FAULT1_INT] ? 1 : 2);; - } else { - /* it's safe to call the following functions even if motor was not running */ - moto_drv8424_set_power(md, 0); - if (md->power_default_off) { - dev_info(md->dev, "vdd power off\n"); - moto_drv8424_set_regulator_power(md, false); - } + /* this will stop polling */ + atomic_set(&md->destination, 0); - /* if position detected based on sensor data, - * current position will be set to destination - * to follow the same logic flow - */ - atomic_set(&md->position, atomic_read(&md->destination)); - moto_drv8424_cmd_push(md, CMD_POSITION, 0); - - /* this will stop polling */ - atomic_set(&md->destination, 0); - - switch (atomic_read(&md->position)) { - case POS_COMPACT: + switch (atomic_read(&md->position)) { + case POS_COMPACT: value = STATUS_STOPPED_COMPACT; break; - case POS_EXPANDED: + case POS_EXPANDED: value = STATUS_STOPPED_EXPANDED; break; - case POS_PEEK: + case POS_PEEK: value = STATUS_STOPPED_PEEK; break; - default: value = STATUS_UNKNOWN; + default: value = STATUS_UNKNOWN; + } + + atomic_set(&md->status, value); + moto_drv8424_cmd_push(md, CMD_STATUS, 0); + + LOGD("Status updated: status %d, position %d\n", + atomic_read(&md->status), atomic_read(&md->position)); + logtime_show(); } - - atomic_set(&md->status, value); - moto_drv8424_cmd_push(md, CMD_STATUS, 0); - - LOGD("Status updated: status %d, position %d\n", - atomic_read(&md->status), atomic_read(&md->position)); - logtime_show(); } - } - return 0; + return 0; } static int disable_motor(struct device* dev) { - motor_device* md = (motor_device*)dev_get_drvdata(dev); - ktime_t time_rem; - int ret = 0; + motor_device* md = (motor_device*)dev_get_drvdata(dev); + ktime_t time_rem; + int ret = 0; - if(atomic_read(&md->stepping)) { - atomic_set(&md->stepping, 0); - atomic_set(&md->step_count, 0); + if (atomic_read(&md->stepping)) { + atomic_set(&md->stepping, 0); + atomic_set(&md->step_count, 0); + time_rem = hrtimer_get_remaining(&md->stepping_timer); + if (ktime_to_us(time_rem) > 0) { + hrtimer_try_to_cancel(&md->stepping_timer); + } + } - time_rem = hrtimer_get_remaining(&md->stepping_timer); - if(ktime_to_us(time_rem) > 0) { - hrtimer_try_to_cancel(&md->stepping_timer); - } - } - - return ret; + return ret; } static int motor_set_enable(struct device* dev, bool enable) { - motor_device* md = (motor_device*)dev_get_drvdata(dev); + motor_device* md = (motor_device*)dev_get_drvdata(dev); - if(atomic_read(&md->stepping)) { - disable_motor(dev); - } + if (atomic_read(&md->stepping)) { + disable_motor(dev); + } + if (enable) { + moto_drv8424_drive_sequencer(md); + } - if(enable) { - moto_drv8424_drive_sequencer(md); - } - - return 0; + return 0; } static void moto_drv8424_set_enable_with_power(motor_device *md, bool enable) { - if (md->power_default_off && enable) { - dev_info(md->dev, "vdd power on\n"); - moto_drv8424_set_regulator_power(md, true); - msleep(1); - } - - if(!moto_drv8424_set_power(md, enable)) { - motor_set_enable(md->dev, enable); - } + if (md->power_default_off && enable) { + dev_info(md->dev, "vdd power on\n"); + moto_drv8424_set_regulator_power(md, true); + msleep(1); + } + if (!moto_drv8424_set_power(md, enable)) { + motor_set_enable(md->dev, enable); + } } static void motor_set_motion_params(motor_device *md, int start, int end) @@ -1050,27 +1023,23 @@ static void motor_set_motion_params(motor_device *md, int start, int end) static enum hrtimer_restart motor_stepping_timer_action(struct hrtimer *h) { - motor_device * md = container_of(h, motor_device, stepping_timer); - unsigned long flags; - enum hrtimer_restart ret = HRTIMER_RESTART; + motor_device * md = container_of(h, motor_device, stepping_timer); + unsigned long flags; + enum hrtimer_restart ret = HRTIMER_RESTART; - spin_lock_irqsave(&md->mlock, flags); - - if(!atomic_read(&md->stepping)) - goto next_stage; - - md->level = !md->level; - GPIO_OUTPUT_DIR(md->mc.ptable[MOTOR_STEP], md->level); - - if(md->double_edge) { - atomic_inc(&md->step_count); - } else if(md->level) { - atomic_inc(&md->step_count); - } + spin_lock_irqsave(&md->mlock, flags); + if (!atomic_read(&md->stepping)) + goto next_stage; + md->level = !md->level; + GPIO_OUTPUT_DIR(md->mc.ptable[MOTOR_STEP], md->level); + if (md->double_edge) { + atomic_inc(&md->step_count); + } else if (md->level) { + atomic_inc(&md->step_count); + } logtime_store(); - - if(md->step_ceiling && atomic_read(&md->step_count) > md->step_ceiling) { + if (md->step_ceiling && atomic_read(&md->step_count) > md->step_ceiling) { next_stage: /* multi stage sequence */ if (!moto_drv8424_next_stage(md)) { @@ -1079,12 +1048,11 @@ next_stage: spin_unlock_irqrestore(&md->mlock, flags); return HRTIMER_NORESTART; } - } + } + hrtimer_forward_now(h, adapt_time_helper(md->half)); + spin_unlock_irqrestore(&md->mlock, flags); - hrtimer_forward_now(h, adapt_time_helper(md->half)); - spin_unlock_irqrestore(&md->mlock, flags); - - return ret; + return ret; } /* @@ -1094,10 +1062,10 @@ next_stage: */ static enum hrtimer_restart motor_timeout_timer_action(struct hrtimer *h) { - motor_device * md = container_of(h, motor_device, stepping_timer); + motor_device * md = container_of(h, motor_device, stepping_timer); int original = atomic_read(&md->position); int destination = atomic_read(&md->destination); - enum hrtimer_restart ret = HRTIMER_NORESTART; + enum hrtimer_restart ret = HRTIMER_NORESTART; LOGD("Timeout: position=%d, detection=%d\n", original, destination); #if 0 @@ -1122,7 +1090,7 @@ static enum hrtimer_restart motor_timeout_timer_action(struct hrtimer *h) moto_drv8424_set_enable_with_power(md, true); } - return ret; + return ret; } #define OUT_OF_RANGE 10000 @@ -1154,9 +1122,9 @@ static int moto_drv8424_detect_position(motor_device *md) ret = i; atomic_set(&md->stepping, 0); LOGD("sensor within range: (%d) < %d < (%d)\n", - scanner[i].value - PROXIMITY, - md->sensor_data[scanner[i].index], - scanner[i].value + PROXIMITY); + scanner[i].value - PROXIMITY, + md->sensor_data[scanner[i].index], + scanner[i].value + PROXIMITY); break; // assuming position is exclusive!!! } } @@ -1189,11 +1157,11 @@ static void motor_fault_handler(motor_device *md) motor_reset(md); if (md->irq_enabled == false) { - if (gpio_is_valid(md->mc.ptable[MOTOR_FAULT1_INT])) - enable_irq(gpio_to_irq(md->mc.ptable[MOTOR_FAULT1_INT])); - if (gpio_is_valid(md->mc.ptable[MOTOR_FAULT2_INT])) - enable_irq(gpio_to_irq(md->mc.ptable[MOTOR_FAULT2_INT])); - md->irq_enabled = true; + if (gpio_is_valid(md->mc.ptable[MOTOR_FAULT1_INT])) + enable_irq(gpio_to_irq(md->mc.ptable[MOTOR_FAULT1_INT])); + if (gpio_is_valid(md->mc.ptable[MOTOR_FAULT2_INT])) + enable_irq(gpio_to_irq(md->mc.ptable[MOTOR_FAULT2_INT])); + md->irq_enabled = true; } position = atomic_read(&md->position); @@ -1212,7 +1180,7 @@ static void motor_fault_handler(motor_device *md) static void motor_cmd_work(struct work_struct *work) { struct delayed_work *dw = container_of(work, struct delayed_work, work); - motor_device* md = container_of(dw, motor_device, motor_work); + motor_device* md = container_of(dw, motor_device, motor_work); bool polling = false; int ret; int next_cmd_delay = 0; @@ -1227,23 +1195,23 @@ static void motor_cmd_work(struct work_struct *work) moto_drv8424_set_power(md, 1); next_cmd = CMD_PWR_OFF; next_cmd_delay = RESET_TIME; - break; + break; case CMD_PWR_OFF: moto_drv8424_set_power(md, 0); - break; + break; case CMD_FAULT: motor_fault_handler(md); - break; + break; case CMD_STATUS: sysfs_notify(&md->dev->kobj, NULL, "status"); md->status_update_ready = true; wake_up(&md->status_wait); - break; + break; case CMD_POSITION: sysfs_notify(&md->dev->kobj, NULL, "position"); md->position_update_ready = true; wake_up(&md->position_wait); - break; + break; case CMD_POLL: ret = moto_drv8424_detect_position(md); if (ret == POS_UNKNOWN) { @@ -1261,10 +1229,10 @@ static void motor_cmd_work(struct work_struct *work) motor_stop(md, false); } } - break; + break; default: dev_err(md->dev, "Unsupported command %d\n", cmd); - break; + break; } } @@ -1279,371 +1247,333 @@ static void motor_cmd_work(struct work_struct *work) static irqreturn_t motor_fault_irq(int irq, void *pdata) { - motor_device * md = (motor_device*) pdata; - - atomic_set(&md->fault_irq, irq); - /* no need to wake up twice if device is faulting */ - if (!md->faulting) { - md->user_sync_complete = true; - wake_up(&md->sync_complete); - } - md->faulting = true; - - if (md->irq_enabled) { - /* disable both IRQ here and enable them both in fault handler */ - if (gpio_is_valid(md->mc.ptable[MOTOR_FAULT1_INT])) - disable_irq(gpio_to_irq(md->mc.ptable[MOTOR_FAULT1_INT])); - if (gpio_is_valid(md->mc.ptable[MOTOR_FAULT2_INT])) - disable_irq(gpio_to_irq(md->mc.ptable[MOTOR_FAULT2_INT])); - md->irq_enabled = false; - } - - return IRQ_HANDLED; + motor_device * md = (motor_device*) pdata; + atomic_set(&md->fault_irq, irq); + /* no need to wake up twice if device is faulting */ + if (!md->faulting) { + md->user_sync_complete = true; + wake_up(&md->sync_complete); + } + md->faulting = true; + if (md->irq_enabled) { + /* disable both IRQ here and enable them both in fault handler */ + if (gpio_is_valid(md->mc.ptable[MOTOR_FAULT1_INT])) + disable_irq(gpio_to_irq(md->mc.ptable[MOTOR_FAULT1_INT])); + if (gpio_is_valid(md->mc.ptable[MOTOR_FAULT2_INT])) + disable_irq(gpio_to_irq(md->mc.ptable[MOTOR_FAULT2_INT])); + md->irq_enabled = false; + } + return IRQ_HANDLED; } //module node interface static void moto_drv8424_set_torque(motor_device* md, unsigned torque) { - unsigned long flags; + unsigned long flags; - spin_lock_irqsave(&md->mlock, flags); - if(md->torque == torque) { - dev_info(md->dev, "Unchanged the torque, ignore\n"); - goto exit; - } - - md->torque = torque; + spin_lock_irqsave(&md->mlock, flags); + if(md->torque == torque) { + dev_info(md->dev, "Unchanged the torque, ignore\n"); + goto exit; + } + md->torque = torque; exit: - spin_unlock_irqrestore(&md->mlock, flags); + spin_unlock_irqrestore(&md->mlock, flags); } static int moto_drv8424_set_mode(motor_device* md, unsigned mode) { - unsigned long flags; - int ret = 0; - - spin_lock_irqsave(&md->mlock, flags); - if(md->mode == mode) { - dev_info(md->dev, "Unchanged the mode, ignore\n"); - ret = -EINVAL; - goto exit; - } - - md->mode = mode; - spin_unlock_irqrestore(&md->mlock, flags); - - return 0; + unsigned long flags; + int ret = 0; + spin_lock_irqsave(&md->mlock, flags); + if (md->mode == mode) { + dev_info(md->dev, "Unchanged the mode, ignore\n"); + ret = -EINVAL; + goto exit; + } + md->mode = mode; + spin_unlock_irqrestore(&md->mlock, flags); + return 0; exit: - spin_unlock_irqrestore(&md->mlock, flags); - return ret; + spin_unlock_irqrestore(&md->mlock, flags); + return ret; } static int moto_drv8424_set_dir(motor_device* md, unsigned dir) { - unsigned long flags; - int ret = 0; - - spin_lock_irqsave(&md->mlock, flags); - if(md->dir == dir) { - dev_info(md->dev, "Unchanged the dir, ignore\n"); - ret = -EINVAL; - goto exit; - } - - md->dir = dir; - spin_unlock_irqrestore(&md->mlock, flags); - - return 0; + unsigned long flags; + int ret = 0; + spin_lock_irqsave(&md->mlock, flags); + if (md->dir == dir) { + dev_info(md->dev, "Unchanged the dir, ignore\n"); + ret = -EINVAL; + goto exit; + } + md->dir = dir; + spin_unlock_irqrestore(&md->mlock, flags); + return 0; exit: - spin_unlock_irqrestore(&md->mlock, flags); - return ret; + spin_unlock_irqrestore(&md->mlock, flags); + return ret; } //Spec step max frequency 250KHz #define STEP_MAX_FREQ 250000 static void moto_drv8424_set_step_freq(motor_device* md, unsigned freq) { - if(md->step_freq == freq) { - dev_err(md->dev, "Unchanged the freq, ignore\n"); + if (md->step_freq == freq) { + dev_err(md->dev, "Unchanged the freq, ignore\n"); return; - } else if(freq == 0) { - dev_err(md->dev, "Invalid frequency, ignore\n"); + } else if(freq == 0) { + dev_err(md->dev, "Invalid frequency, ignore\n"); return; - } - - if(freq > STEP_MAX_FREQ) - freq = STEP_MAX_FREQ; - md->step_freq = freq; - - md->step_period = 1000000 / md->step_freq; + } + if(freq > STEP_MAX_FREQ) + freq = STEP_MAX_FREQ; + md->step_freq = freq; + md->step_period = 1000000 / md->step_freq; } static int moto_drv8424_set_ceiling(motor_device* md, unsigned ceiling) { - unsigned long flags; - int ret = 0; - - spin_lock_irqsave(&md->mlock, flags); - if(md->step_ceiling == ceiling) { - dev_info(md->dev, "Unchanged the ceiling, ignore\n"); - ret = -EINVAL; - goto exit; - } - - md->step_ceiling = ceiling; - spin_unlock_irqrestore(&md->mlock, flags); - - return 0; + unsigned long flags; + int ret = 0; + spin_lock_irqsave(&md->mlock, flags); + if (md->step_ceiling == ceiling) { + dev_info(md->dev, "Unchanged the ceiling, ignore\n"); + ret = -EINVAL; + goto exit; + } + md->step_ceiling = ceiling; + spin_unlock_irqrestore(&md->mlock, flags); + return 0; exit: - spin_unlock_irqrestore(&md->mlock, flags); - return ret; + spin_unlock_irqrestore(&md->mlock, flags); + return ret; } static ssize_t motor_enable_show(struct device *dev, struct device_attribute *attr, - char *buf) + char *buf) { - motor_device* md = (motor_device*)dev_get_drvdata(dev); - - return snprintf(buf, 20, "%d\n", md->power_en); + motor_device* md = (motor_device*)dev_get_drvdata(dev); + return snprintf(buf, 20, "%d\n", md->power_en); } static ssize_t motor_enable_store(struct device *dev, struct device_attribute *attr, - const char *buf, size_t len) + const char *buf, size_t len) { - motor_device* md = (motor_device*)dev_get_drvdata(dev); - unsigned enable = 0; + motor_device* md = (motor_device*)dev_get_drvdata(dev); + unsigned enable = 0; - if(md->faulting) { - dev_err(dev, "Device faulting\n"); - return -EBUSY; - } - - if(kstrtouint(buf, 10, &enable)) { - dev_err(dev, "Error value: %s\n", buf); - goto exit; - } + if (md->faulting) { + dev_err(dev, "Device faulting\n"); + return -EBUSY; + } + if (kstrtouint(buf, 10, &enable)) { + dev_err(dev, "Error value: %s\n", buf); + goto exit; + } moto_drv8424_set_enable_with_power(md, !!enable); exit: - return len; + return len; } static ssize_t motor_dir_show(struct device *dev, struct device_attribute *attr, - char *buf) + char *buf) { - motor_device* md = (motor_device*)dev_get_drvdata(dev); - - return snprintf(buf, 20, "%d\n", md->dir); + motor_device* md = (motor_device*)dev_get_drvdata(dev); + return snprintf(buf, 20, "%d\n", md->dir); } static ssize_t motor_dir_store(struct device *dev, struct device_attribute *attr, - const char *buf, size_t len) + const char *buf, size_t len) { - motor_device* md = (motor_device*)dev_get_drvdata(dev); - unsigned value = 0; - - if(md->faulting) { - dev_err(dev, "Device faulting\n"); - return -EBUSY; - } - - if(kstrtouint(buf, 10, &value)) { - dev_err(dev, "Error value: %s\n", buf); - goto exit; - } - - moto_drv8424_set_dir(md, value); + motor_device* md = (motor_device*)dev_get_drvdata(dev); + unsigned value = 0; + if (md->faulting) { + dev_err(dev, "Device faulting\n"); + return -EBUSY; + } + if (kstrtouint(buf, 10, &value)) { + dev_err(dev, "Error value: %s\n", buf); + goto exit; + } + moto_drv8424_set_dir(md, value); exit: - return len; + return len; } static ssize_t motor_step_show(struct device *dev, struct device_attribute *attr, - char *buf) + char *buf) { - motor_device* md = (motor_device*)dev_get_drvdata(dev); - - return snprintf(buf, 20, "%d\n", md->step_freq); + motor_device* md = (motor_device*)dev_get_drvdata(dev); + return snprintf(buf, 20, "%d\n", md->step_freq); } static ssize_t motor_step_store(struct device *dev, struct device_attribute *attr, - const char *buf, size_t len) + const char *buf, size_t len) { - motor_device* md = (motor_device*)dev_get_drvdata(dev); + motor_device* md = (motor_device*)dev_get_drvdata(dev); unsigned long flags; - unsigned value = 0; + unsigned value = 0; - if(md->faulting) { - dev_err(dev, "Device faulting\n"); - return -EBUSY; - } + if (md->faulting) { + dev_err(dev, "Device faulting\n"); + return -EBUSY; + } + if (kstrtouint(buf, 10, &value)) { + dev_err(dev, "Error value: %s\n", buf); + return -EINVAL; + } + spin_lock_irqsave(&md->mlock, flags); + moto_drv8424_set_step_freq(md, value); + spin_unlock_irqrestore(&md->mlock, flags); + dev_info(md->dev, "freq %uHz period %ldus\n", md->step_freq, md->step_period); - if(kstrtouint(buf, 10, &value)) { - dev_err(dev, "Error value: %s\n", buf); - return -EINVAL; - } - spin_lock_irqsave(&md->mlock, flags); - moto_drv8424_set_step_freq(md, value); - spin_unlock_irqrestore(&md->mlock, flags); - dev_info(md->dev, "freq %uHz period %ldus\n", md->step_freq, md->step_period); - - return len; + return len; } static ssize_t motor_mode_show(struct device *dev, struct device_attribute *attr, - char *buf) + char *buf) { - motor_device* md = (motor_device*)dev_get_drvdata(dev); - - return snprintf(buf, 20, "%d\n", md->mode); + motor_device* md = (motor_device*)dev_get_drvdata(dev); + return snprintf(buf, 20, "%d\n", md->mode); } static ssize_t motor_mode_store(struct device *dev, struct device_attribute *attr, - const char *buf, size_t len) + const char *buf, size_t len) { - motor_device* md = (motor_device*)dev_get_drvdata(dev); - unsigned value = 0; - - if(md->faulting) { - dev_err(dev, "Device faulting\n"); - return -EBUSY; - } - - if(kstrtouint(buf, 10, &value)) { - dev_err(dev, "Error value: %s\n", buf); - goto exit; - } - - moto_drv8424_set_mode(md, value); + motor_device* md = (motor_device*)dev_get_drvdata(dev); + unsigned value = 0; + if (md->faulting) { + dev_err(dev, "Device faulting\n"); + return -EBUSY; + } + if (kstrtouint(buf, 10, &value)) { + dev_err(dev, "Error value: %s\n", buf); + goto exit; + } + moto_drv8424_set_mode(md, value); exit: - return len; + return len; } static ssize_t motor_torque_show(struct device *dev, struct device_attribute *attr, - char *buf) + char *buf) { - motor_device* md = (motor_device*)dev_get_drvdata(dev); - - return snprintf(buf, 20, "%d\n", md->torque); + motor_device* md = (motor_device*)dev_get_drvdata(dev); + return snprintf(buf, 20, "%d\n", md->torque); } static ssize_t motor_torque_store(struct device *dev, struct device_attribute *attr, - const char *buf, size_t len) + const char *buf, size_t len) { - motor_device* md = (motor_device*)dev_get_drvdata(dev); - unsigned value = 0; - - if(md->faulting) { - dev_err(dev, "Device faulting\n"); - return -EBUSY; - } - - if(kstrtouint(buf, 10, &value)) { - dev_err(dev, "Error value: %s\n", buf); - goto exit; - } - - moto_drv8424_set_torque(md, value); + motor_device* md = (motor_device*)dev_get_drvdata(dev); + unsigned value = 0; + if (md->faulting) { + dev_err(dev, "Device faulting\n"); + return -EBUSY; + } + if (kstrtouint(buf, 10, &value)) { + dev_err(dev, "Error value: %s\n", buf); + goto exit; + } + moto_drv8424_set_torque(md, value); exit: - return len; + return len; } static ssize_t motor_ceiling_show(struct device *dev, struct device_attribute *attr, - char *buf) + char *buf) { - motor_device* md = (motor_device*)dev_get_drvdata(dev); - - return snprintf(buf, 20, "%ld\n", md->step_ceiling); + motor_device* md = (motor_device*)dev_get_drvdata(dev); + return snprintf(buf, 20, "%ld\n", md->step_ceiling); } static ssize_t motor_ceiling_store(struct device *dev, struct device_attribute *attr, - const char *buf, size_t len) + const char *buf, size_t len) { - motor_device* md = (motor_device*)dev_get_drvdata(dev); - unsigned value = 0; - - if(md->faulting) { - dev_err(dev, "Device faulting\n"); - return -EBUSY; - } - - if(kstrtouint(buf, 10, &value)) { - dev_err(dev, "Error value: %s\n", buf); - goto exit; - } - - moto_drv8424_set_ceiling(md, value); + motor_device* md = (motor_device*)dev_get_drvdata(dev); + unsigned value = 0; + if (md->faulting) { + dev_err(dev, "Device faulting\n"); + return -EBUSY; + } + if (kstrtouint(buf, 10, &value)) { + dev_err(dev, "Error value: %s\n", buf); + goto exit; + } + moto_drv8424_set_ceiling(md, value); exit: - return len; + return len; } static ssize_t motor_time_out_show(struct device *dev, struct device_attribute *attr, - char *buf) + char *buf) { - motor_device* md = (motor_device*)dev_get_drvdata(dev); - - return snprintf(buf, 20, "%d\n", md->time_out); + motor_device* md = (motor_device*)dev_get_drvdata(dev); + return snprintf(buf, 20, "%d\n", md->time_out); } static ssize_t motor_time_out_store(struct device *dev, struct device_attribute *attr, - const char *buf, size_t len) + const char *buf, size_t len) { - motor_device* md = (motor_device*)dev_get_drvdata(dev); - unsigned value = 0; - unsigned long flags; - - if(md->faulting) { - dev_err(dev, "Device faulting\n"); - return -EBUSY; - } - - if(kstrtouint(buf, 10, &value)) { - dev_err(dev, "Error value: %s\n", buf); - goto exit; - } - - spin_lock_irqsave(&md->mlock, flags); - if(value > MOTOR_DEFAULT_EXPIRE) { - value = MOTOR_DEFAULT_EXPIRE; - } - md->time_out = value; - spin_unlock_irqrestore(&md->mlock, flags); + motor_device* md = (motor_device*)dev_get_drvdata(dev); + unsigned value = 0; + unsigned long flags; + if (md->faulting) { + dev_err(dev, "Device faulting\n"); + return -EBUSY; + } + if (kstrtouint(buf, 10, &value)) { + dev_err(dev, "Error value: %s\n", buf); + goto exit; + } + spin_lock_irqsave(&md->mlock, flags); + if (value > MOTOR_DEFAULT_EXPIRE) { + value = MOTOR_DEFAULT_EXPIRE; + } + md->time_out = value; + spin_unlock_irqrestore(&md->mlock, flags); exit: - return len; + return len; } static ssize_t motor_reset_store(struct device *dev, struct device_attribute *attr, - const char *buf, size_t len) + const char *buf, size_t len) { - motor_device* md = (motor_device*)dev_get_drvdata(dev); - unsigned value = 0; + motor_device* md = (motor_device*)dev_get_drvdata(dev); + unsigned value = 0; - if (kstrtouint(buf, 10, &value)) { - dev_err(dev, "Error value: %s\n", buf); - } else { - mutex_lock(&md->mx_lock); - motor_reset(md); - mutex_unlock(&md->mx_lock); - dev_info(dev, "Device reset\n"); - } - return len; + if (kstrtouint(buf, 10, &value)) { + dev_err(dev, "Error value: %s\n", buf); + } else { + mutex_lock(&md->mx_lock); + motor_reset(md); + mutex_unlock(&md->mx_lock); + dev_info(dev, "Device reset\n"); + } + + return len; } /* store new motor sequencer */ /* Format: index freq0:ceiling0 [... freqN:ceilingN] */ /* Example: 0 400:24 1400:56 3000:6252 2000:12 */ static ssize_t motor_sequencer_store(struct device *dev, struct device_attribute *attr, - const char *buf, size_t len) + const char *buf, size_t len) { - motor_device* md = (motor_device*)dev_get_drvdata(dev); + motor_device* md = (motor_device*)dev_get_drvdata(dev); char *buffer, *next, *pair; - unsigned nstages = 0; + unsigned nstages = 0; unsigned msn, value, idx, freq, ceiling; buffer = kmalloc(len + 1, GFP_KERNEL); @@ -1659,11 +1589,11 @@ static ssize_t motor_sequencer_store(struct device *dev, struct device_attribute goto exit; } *next++ = '\0'; - if(kstrtouint(buffer, 10, &value) || - (value < 0 || value >= SQ_MAX)) { - dev_err(dev, "Invalid index: %s\n", buffer); - goto exit; - } + if (kstrtouint(buffer, 10, &value) || + (value < 0 || value >= SQ_MAX)) { + dev_err(dev, "Invalid index: %s\n", buffer); + goto exit; + } idx = value; for (msn = 0, pair = next; msn < MAX_STAGE_LEGS; msn++) { LOGD("old values [%u][%u]: freq=%u, ceiling=%u\n", @@ -1690,7 +1620,7 @@ static ssize_t motor_sequencer_store(struct device *dev, struct device_attribute idx, msn, freq, ceiling); } pair = next; - } + } if (nstages > 0) { md->max_stages = nstages; @@ -1702,13 +1632,13 @@ static ssize_t motor_sequencer_store(struct device *dev, struct device_attribute exit: kfree(buffer); - return len; + return len; } static ssize_t motor_sequencer_show(struct device *dev, struct device_attribute *attr, - char *buf) + char *buf) { - motor_device* md = (motor_device*)dev_get_drvdata(dev); + motor_device* md = (motor_device*)dev_get_drvdata(dev); ssize_t blen = 0; unsigned idx, msn; @@ -1719,82 +1649,77 @@ static ssize_t motor_sequencer_show(struct device *dev, struct device_attribute if (md->sequencer[idx][msn].freq == 0 && md->sequencer[idx][msn].ceiling == 0) continue; - blen += snprintf(buf + blen, PAGE_SIZE - blen, "%u:%u ", - md->sequencer[idx][msn].freq, - md->sequencer[idx][msn].ceiling); + md->sequencer[idx][msn].freq, + md->sequencer[idx][msn].ceiling); } blen += snprintf(buf + blen, PAGE_SIZE - blen, "\n"); } - return blen; + return blen; } static ssize_t motor_regime_store(struct device *dev, struct device_attribute *attr, - const char *buf, size_t len) + const char *buf, size_t len) { - motor_device* md = (motor_device*)dev_get_drvdata(dev); + motor_device* md = (motor_device*)dev_get_drvdata(dev); unsigned value; - if(kstrtouint(buf, 10, &value) || + if (kstrtouint(buf, 10, &value) || (value < SQ_FULL || value >= SQ_MAX)) { - dev_err(dev, "Error value: %s\n", buf); - return -EINVAL; - } + dev_err(dev, "Error value: %s\n", buf); + return -EINVAL; + } + moto_drv8424_set_regime(md, value); - moto_drv8424_set_regime(md, value); - - return len; + return len; } static ssize_t motor_regime_show(struct device *dev, struct device_attribute *attr, - char *buf) + char *buf) { - motor_device* md = (motor_device*)dev_get_drvdata(dev); - - return snprintf(buf, 20, "%s\n", regime_names[md->regime]); + motor_device* md = (motor_device*)dev_get_drvdata(dev); + return snprintf(buf, 20, "%s\n", regime_names[md->regime]); } static ssize_t motor_irq_store(struct device *dev, struct device_attribute *attr, - const char *buf, size_t len) + const char *buf, size_t len) { - motor_device* md = (motor_device*)dev_get_drvdata(dev); - unsigned value; + motor_device* md = (motor_device*)dev_get_drvdata(dev); + unsigned value; - if(kstrtouint(buf, 10, &value)) { - dev_err(dev, "Error value: %s\n", buf); - return -EINVAL; - } - - if (!!value) { - if (md->irq_enabled == false) { - if (gpio_is_valid(md->mc.ptable[MOTOR_FAULT1_INT])) - enable_irq(gpio_to_irq(md->mc.ptable[MOTOR_FAULT1_INT])); - if (gpio_is_valid(md->mc.ptable[MOTOR_FAULT2_INT])) - enable_irq(gpio_to_irq(md->mc.ptable[MOTOR_FAULT2_INT])); - md->irq_enabled = true; - dev_info(dev, "Enabled fault IRQ\n"); + if (kstrtouint(buf, 10, &value)) { + dev_err(dev, "Error value: %s\n", buf); + return -EINVAL; } - } else { - if (md->irq_enabled == true) { - if (gpio_is_valid(md->mc.ptable[MOTOR_FAULT1_INT])) - disable_irq(gpio_to_irq(md->mc.ptable[MOTOR_FAULT1_INT])); - if (gpio_is_valid(md->mc.ptable[MOTOR_FAULT2_INT])) - disable_irq(gpio_to_irq(md->mc.ptable[MOTOR_FAULT2_INT])); - md->irq_enabled = false; - dev_info(dev, "Disabled fault IRQ\n"); + if (!!value) { + if (md->irq_enabled == false) { + if (gpio_is_valid(md->mc.ptable[MOTOR_FAULT1_INT])) + enable_irq(gpio_to_irq(md->mc.ptable[MOTOR_FAULT1_INT])); + if (gpio_is_valid(md->mc.ptable[MOTOR_FAULT2_INT])) + enable_irq(gpio_to_irq(md->mc.ptable[MOTOR_FAULT2_INT])); + md->irq_enabled = true; + dev_info(dev, "Enabled fault IRQ\n"); + } + } else { + if (md->irq_enabled == true) { + if (gpio_is_valid(md->mc.ptable[MOTOR_FAULT1_INT])) + disable_irq(gpio_to_irq(md->mc.ptable[MOTOR_FAULT1_INT])); + if (gpio_is_valid(md->mc.ptable[MOTOR_FAULT2_INT])) + disable_irq(gpio_to_irq(md->mc.ptable[MOTOR_FAULT2_INT])); + md->irq_enabled = false; + dev_info(dev, "Disabled fault IRQ\n"); + } } - } - return len; + return len; } static ssize_t motor_irq_show(struct device *dev, struct device_attribute *attr, - char *buf) + char *buf) { - motor_device* md = (motor_device*)dev_get_drvdata(dev); - - return sprintf(buf, "%d\n", md->irq_enabled ? 1 : 0); + motor_device* md = (motor_device*)dev_get_drvdata(dev); + return sprintf(buf, "%d\n", md->irq_enabled ? 1 : 0); } static DEVICE_ATTR(enable, S_IRUGO|S_IWUSR|S_IWGRP, motor_enable_show, motor_enable_store); @@ -1815,16 +1740,16 @@ static DEVICE_ATTR(irq, S_IRUGO|S_IWUSR|S_IWGRP, motor_irq_show, motor_irq_store static int last_idx = ATTRS_STATIC; static struct attribute *motor_attributes[ATTRS_MAX + 1] = { - &dev_attr_dir.attr, - &dev_attr_enable.attr, - &dev_attr_step.attr, - &dev_attr_ceiling.attr, - &dev_attr_reset.attr, - &dev_attr_time_out.attr, - &dev_attr_regime.attr, - &dev_attr_sequencer.attr, - &dev_attr_irq.attr, - NULL + &dev_attr_dir.attr, + &dev_attr_enable.attr, + &dev_attr_step.attr, + &dev_attr_ceiling.attr, + &dev_attr_reset.attr, + &dev_attr_time_out.attr, + &dev_attr_regime.attr, + &dev_attr_sequencer.attr, + &dev_attr_irq.attr, + NULL }; #define ATTR_ADD(name) { \ @@ -1837,41 +1762,40 @@ static struct attribute *motor_attributes[ATTRS_MAX + 1] = { } static struct attribute_group motor_attribute_group = { - .attrs = motor_attributes + .attrs = motor_attributes }; static const struct attribute_group * motor_attr_groups[] = { - &motor_attribute_group, - NULL + &motor_attribute_group, + NULL }; /* HAL communicates desired position through this sysfs */ static ssize_t motor_position_store(struct device *dev, struct device_attribute *attr, - const char *buf, size_t len) + const char *buf, size_t len) { - motor_device* md = (motor_device*)dev_get_drvdata(dev); - unsigned long flags; - unsigned value = 0; + motor_device* md = (motor_device*)dev_get_drvdata(dev); + unsigned long flags; + unsigned value = 0; - if(kstrtouint(buf, 10, &value) || + if (kstrtouint(buf, 10, &value) || /* at this point it's well known position */ (value <= POS_UNKNOWN || value > POS_PEEK)) { - dev_err(dev, "Error value: %s\n", buf); - return -EINVAL; - } - - spin_lock_irqsave(&md->mlock, flags); + dev_err(dev, "Error value: %s\n", buf); + return -EINVAL; + } + spin_lock_irqsave(&md->mlock, flags); atomic_set(&md->destination, value); - spin_unlock_irqrestore(&md->mlock, flags); + spin_unlock_irqrestore(&md->mlock, flags); dev_info(md->dev, "expected position %s\n", position_labels[value]); - return len; + return len; } static ssize_t motor_position_show(struct device *dev, struct device_attribute *attr, - char *buf) + char *buf) { - motor_device* md = (motor_device*)dev_get_drvdata(dev); + motor_device* md = (motor_device*)dev_get_drvdata(dev); int ret; ret = wait_event_interruptible(md->position_wait, md->position_update_ready); @@ -1880,13 +1804,13 @@ static ssize_t motor_position_show(struct device *dev, struct device_attribute * md->position_update_ready = false; LOGD("Position update: %s\n", position_labels[atomic_read(&md->position)]); - return snprintf(buf, 20, "%d", atomic_read(&md->position)); + return snprintf(buf, 20, "%d", atomic_read(&md->position)); } static ssize_t motor_status_show(struct device *dev, struct device_attribute *attr, - char *buf) + char *buf) { - motor_device* md = (motor_device*)dev_get_drvdata(dev); + motor_device* md = (motor_device*)dev_get_drvdata(dev); int ret; ret = wait_event_interruptible(md->status_wait, md->status_update_ready); @@ -1895,7 +1819,7 @@ static ssize_t motor_status_show(struct device *dev, struct device_attribute *at md->status_update_ready = false; LOGD("Status update: %s\n", status_labels[atomic_read(&md->status)]); - return snprintf(buf, 20, "%d", atomic_read(&md->status)); + return snprintf(buf, 20, "%d", atomic_read(&md->status)); } /* * Input data expected in the following format: @@ -1903,10 +1827,10 @@ static ssize_t motor_status_show(struct device *dev, struct device_attribute *at * where Decimal_3 is optional */ static ssize_t motor_sensor_data_store(struct device *dev, struct device_attribute *attr, - const char *buf, size_t len) + const char *buf, size_t len) { - motor_device* md = (motor_device*)dev_get_drvdata(dev); - int argnum = 0; + motor_device* md = (motor_device*)dev_get_drvdata(dev); + int argnum = 0; int args[3] = {OUT_OF_RANGE}; #if 0 if (atmic_read(&md->destination) == 0) { @@ -1922,9 +1846,8 @@ static ssize_t motor_sensor_data_store(struct device *dev, struct device_attribu memcpy(md->sensor_data, args, sizeof(md->sensor_data)); mutex_unlock(&md->mx_lock); } - //just_exit: - return len; + return len; } /* sysfs polled from HAL */ @@ -1933,105 +1856,100 @@ static DEVICE_ATTR(status, S_IRUGO, motor_status_show, NULL); static DEVICE_ATTR(sensor_data, S_IWUSR|S_IWGRP, NULL, motor_sensor_data_store); static struct attribute *status_attributes[] = { - &dev_attr_status.attr, - &dev_attr_position.attr, - &dev_attr_sensor_data.attr, - NULL + &dev_attr_status.attr, + &dev_attr_position.attr, + &dev_attr_sensor_data.attr, + NULL }; static struct attribute_group status_attribute_group = { - .attrs = status_attributes + .attrs = status_attributes }; static int moto_drv8424_init_from_dt(motor_device* md) { - struct device* pdev = md->dev; - struct device_node *np = pdev->of_node; - motor_control* mc = &md->mc; - int i, rc = 0; + struct device* pdev = md->dev; + struct device_node *np = pdev->of_node; + motor_control* mc = &md->mc; + int i, rc = 0; char gpio_name[32]; - const char *clock_name; + const char *clock_name; - mc->tab_cells = MOTOR_MAX_GPIO; - for (i = 0; i < mc->tab_cells; i++) { - snprintf(gpio_name, sizeof(gpio_name)-1, "%s-gpio", gpios_labels[i]); - md->mc.ptable[i] = of_get_named_gpio(np, gpio_name, 0); - if (gpio_is_valid(md->mc.ptable[i])) { - dev_info(pdev, "gpio %s = %d\n", gpios_labels[i], md->mc.ptable[i]); + mc->tab_cells = MOTOR_MAX_GPIO; + for (i = 0; i < mc->tab_cells; i++) { + snprintf(gpio_name, sizeof(gpio_name)-1, "%s-gpio", gpios_labels[i]); + md->mc.ptable[i] = of_get_named_gpio(np, gpio_name, 0); + if (gpio_is_valid(md->mc.ptable[i])) { + dev_info(pdev, "gpio %s = %d\n", gpios_labels[i], md->mc.ptable[i]); + } } - } + if (!of_property_read_string(np, "clock-names", &clock_name)) + strlcpy(md->clock_name, clock_name, CLOCK_NAME_LEN); + else + strlcpy(md->clock_name, MOTOR_HW_CLK_NAME, CLOCK_NAME_LEN); + dev_dbg(pdev, "Clock name: %s\n", md->clock_name); + md->hw_clock = of_property_read_bool(np, "enable-hw-clock"); + dev_dbg(pdev, "Enable hw clock %d\n", md->hw_clock); + md->support_mode = of_property_read_bool(np, "support-mode"); + if (md->support_mode) { + ATTR_ADD(mode); + dev_info(pdev, "Support mode %d\n", md->support_mode); + } + md->support_torque = of_property_read_bool(np, "support-torque"); + if (md->support_torque) { + ATTR_ADD(torque); + dev_info(pdev, "Support torque %d\n", md->support_torque); + }; + md->sensors_off = of_property_read_bool(np, "no-sensors"); + if (md->sensors_off) + dev_info(pdev, "Ignore sensors data\n"); + md->power_default_off = of_property_read_bool(np, "power-default-off"); + dev_info(pdev, "power is default off: %d\n", md->power_default_off); - if (!of_property_read_string(np, "clock-names", &clock_name)) - strlcpy(md->clock_name, clock_name, CLOCK_NAME_LEN); - else - strlcpy(md->clock_name, MOTOR_HW_CLK_NAME, CLOCK_NAME_LEN); - dev_dbg(pdev, "Clock name: %s\n", md->clock_name); - - md->hw_clock = of_property_read_bool(np, "enable-hw-clock"); - dev_dbg(pdev, "Enable hw clock %d\n", md->hw_clock); - - md->support_mode = of_property_read_bool(np, "support-mode"); - if (md->support_mode) { - ATTR_ADD(mode); - dev_info(pdev, "Support mode %d\n", md->support_mode); - } - - md->support_torque = of_property_read_bool(np, "support-torque"); - if (md->support_torque) { - ATTR_ADD(torque); - dev_info(pdev, "Support torque %d\n", md->support_torque); - }; - - md->sensors_off = of_property_read_bool(np, "no-sensors"); - if (md->sensors_off) - dev_info(pdev, "Ignore sensors data\n"); - - md->power_default_off = of_property_read_bool(np, "power-default-off"); - dev_info(pdev, "power is default off: %d\n", md->power_default_off); - - return rc; + return rc; } static int moto_drv8424_irq_setup(motor_device *md, unsigned gidx) { - static int id; - int ret, irq_gpio = md->mc.ptable[gidx]; - char irq_name[32]; + static int id; + int ret, irq_gpio = md->mc.ptable[gidx]; + char irq_name[32]; - if (!gpio_is_valid(irq_gpio)) - return ENOTCONN; + if (!gpio_is_valid(irq_gpio)) + return ENOTCONN; + snprintf(irq_name, sizeof(irq_name)-1, "fault_motor%d", ++id); + ret = devm_request_threaded_irq(md->dev, + gpio_to_irq(irq_gpio), motor_fault_irq, NULL, + IRQF_TRIGGER_LOW | IRQF_ONESHOT, irq_name, md); + if (ret) { + dev_err(md->dev, "Failed to register \'%s\': %d\n", + irq_name, ret); + } else { + dev_info(md->dev, "Registered IRQ %d as '%s'\n", + gpio_to_irq(irq_gpio), irq_name); + } - snprintf(irq_name, sizeof(irq_name)-1, "fault_motor%d", ++id); - ret = devm_request_threaded_irq(md->dev, - gpio_to_irq(irq_gpio), motor_fault_irq, NULL, - IRQF_TRIGGER_LOW | IRQF_ONESHOT, irq_name, md); - if (ret) - dev_err(md->dev, "Failed to register \'%s\': %d\n", irq_name, ret); - else - dev_info(md->dev, "Registered IRQ %d as '%s'\n", gpio_to_irq(irq_gpio), irq_name); - - return ret; + return ret; } static int moto_drv8424_probe(struct platform_device *pdev) { - struct device* dev = &pdev->dev; - motor_device* md; - int i; - int ret = 0; + struct device* dev = &pdev->dev; + motor_device* md; + int i; + int ret = 0; - md = kzalloc(sizeof(motor_device), GFP_KERNEL); - if(!md) { - dev_err(dev, "probe: Out of memory\n"); - return -ENOMEM; - } - - md->dev = dev; - md->mc.plabels = gpios_labels; - spin_lock_init(&md->mlock); - mutex_init(&md->mx_lock); - platform_set_drvdata(pdev, md); - moto_drv8424_set_step_freq(md, DEFAULT_STEP_FREQ); + md = kzalloc(sizeof(motor_device), GFP_KERNEL); + if (!md) { + dev_err(dev, "probe: Out of memory\n"); + return -ENOMEM; + } + md->dev = dev; + md->mc.plabels = gpios_labels; + spin_lock_init(&md->mlock); + mutex_init(&md->mx_lock); + platform_set_drvdata(pdev, md); + moto_drv8424_set_step_freq(md, DEFAULT_STEP_FREQ); /* populate sequencer with initial values */ for (i = SQ_FULL; i < SQ_MAX; i++) @@ -2040,185 +1958,167 @@ static int moto_drv8424_probe(struct platform_device *pdev) /* assign default values for optional parameters */ md->mode = FULL_STEP; md->torque = TORQUE_FULL; - - moto_drv8424_init_from_dt(md); - md->mc.vdd = devm_regulator_get(md->dev, "vdd"); - if (IS_ERR(md->mc.vdd)) { - ret = PTR_ERR(md->mc.vdd); - dev_err(md->dev, "Failed to get VDD ret=%d\n", ret); - goto failed_mem; - } - - if (!md->power_default_off) { - ret = moto_drv8424_set_regulator_power(md, true); - if(ret) { - regulator_put(md->mc.vdd); - dev_err(dev, "Failed enable regulator\n"); - goto failed_mem; - } - } - - md->motor_wq = alloc_workqueue("motor_wq", WQ_HIGHPRI, 0); - if(!md->motor_wq) { - dev_err(dev, "Out of memory for work queue\n"); - goto failed_mem; - } - INIT_DELAYED_WORK(&md->motor_work, motor_cmd_work); + moto_drv8424_init_from_dt(md); + md->mc.vdd = devm_regulator_get(md->dev, "vdd"); + if (IS_ERR(md->mc.vdd)) { + ret = PTR_ERR(md->mc.vdd); + dev_err(md->dev, "Failed to get VDD ret=%d\n", ret); + goto failed_mem; + } + if (!md->power_default_off) { + ret = moto_drv8424_set_regulator_power(md, true); + if (ret) { + regulator_put(md->mc.vdd); + dev_err(dev, "Failed enable regulator\n"); + goto failed_mem; + } + } + md->motor_wq = alloc_workqueue("motor_wq", WQ_HIGHPRI, 0); + if(!md->motor_wq) { + dev_err(dev, "Out of memory for work queue\n"); + goto failed_mem; + } + INIT_DELAYED_WORK(&md->motor_work, motor_cmd_work); ret = kfifo_alloc(&md->cmd_pipe, sizeof(unsigned int)* 10, GFP_KERNEL); if (ret) { - dev_err(dev, "Failed create fifo\n"); - goto failed_work; + dev_err(dev, "Failed create fifo\n"); + goto failed_work; } - - md->user_sync_complete = false; - init_waitqueue_head(&md->sync_complete); + md->user_sync_complete = false; + init_waitqueue_head(&md->sync_complete); md->status_update_ready = false; - init_waitqueue_head(&md->status_wait); + init_waitqueue_head(&md->status_wait); md->position_update_ready = false; - init_waitqueue_head(&md->position_wait); - - md->motor_task = kthread_create(motor_kthread, md, "motor_task"); - if (IS_ERR(md->motor_task)) { - ret = PTR_ERR(md->motor_task); - dev_err(dev, "Failed create motor kthread\n"); - goto failed_kfifo; - } - - hrtimer_init(&md->stepping_timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL); - md->stepping_timer.function = motor_stepping_timer_action; - - hrtimer_init(&md->timeout_timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL); - md->timeout_timer.function = motor_timeout_timer_action; - - md->drv8424_class = class_create(THIS_MODULE, MOTOR_CLASS_NAME); - if(IS_ERR(md->drv8424_class)) { - dev_err(dev, "Failed to create class\n"); - goto failed_sys; - } - - md->sysfs_dev = device_create_with_groups(md->drv8424_class, - dev, MKDEV(0, 0), md, motor_attr_groups, - "%s", MOTOR_CONTROL); - if(IS_ERR(md->sysfs_dev)) { - dev_err(dev, "Failed to create device\n"); - goto failed_sys_dev; - } - - if(sysfs_create_group(&md->dev->kobj, &status_attribute_group)) { - dev_err(dev, "Failed to create status attribute"); - goto failed_device; - } - - if(md->hw_clock) { - if(init_motor_clk(md) < 0) { - //Should get this gpio from DTB - dev_err(dev, "HW clock is not setup\n"); - } else { - md->cur_clk = hw_clocks[0]; - dev_info(dev, "HW clock is setup\n"); - } - } - - /* set initial pinctrl */ - set_pinctrl_state(md, INIT_STATE); - //Init software clock pin. - set_pinctrl_state(md, CLK_SLEEP); - for (i = 0; i < MOTOR_UNKNOWN; i++) { + init_waitqueue_head(&md->position_wait); + md->motor_task = kthread_create(motor_kthread, md, "motor_task"); + if (IS_ERR(md->motor_task)) { + ret = PTR_ERR(md->motor_task); + dev_err(dev, "Failed create motor kthread\n"); + goto failed_kfifo; + } + hrtimer_init(&md->stepping_timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL); + md->stepping_timer.function = motor_stepping_timer_action; + hrtimer_init(&md->timeout_timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL); + md->timeout_timer.function = motor_timeout_timer_action; + md->drv8424_class = class_create(THIS_MODULE, MOTOR_CLASS_NAME); + if(IS_ERR(md->drv8424_class)) { + dev_err(dev, "Failed to create class\n"); + goto failed_sys; + } + md->sysfs_dev = device_create_with_groups(md->drv8424_class, + dev, MKDEV(0, 0), md, motor_attr_groups, + "%s", MOTOR_CONTROL); + if (IS_ERR(md->sysfs_dev)) { + dev_err(dev, "Failed to create device\n"); + goto failed_sys_dev; + } + if (sysfs_create_group(&md->dev->kobj, &status_attribute_group)) { + dev_err(dev, "Failed to create status attribute"); + goto failed_device; + } + if (md->hw_clock) { + if (init_motor_clk(md) < 0) { + //Should get this gpio from DTB + dev_err(dev, "HW clock is not setup\n"); + } else { + md->cur_clk = hw_clocks[0]; + dev_info(dev, "HW clock is setup\n"); + } + } + /* set initial pinctrl */ + set_pinctrl_state(md, INIT_STATE); + //Init software clock pin. + set_pinctrl_state(md, CLK_SLEEP); + for (i = 0; i < MOTOR_UNKNOWN; i++) { if (!gpio_is_valid(md->mc.ptable[i])) continue; - ret = devm_gpio_request(dev, md->mc.ptable[i], gpios_labels[i]); - if(ret < 0) { - pr_err("Failed to request %s, errno %d\n", gpios_labels[i--], ret); - goto failed_gpio; - } - } + ret = devm_gpio_request(dev, md->mc.ptable[i], gpios_labels[i]); + if(ret < 0) { + pr_err("Failed to request %s, errno %d\n", gpios_labels[i--], ret); + goto failed_gpio; + } + } + wake_up_process(md->motor_task); + if (!set_pinctrl_state(md, INT_DEFAULT)) { + ret = moto_drv8424_irq_setup(md, MOTOR_FAULT1_INT); + if (ret < 0) + goto failed_gpio; + ret = moto_drv8424_irq_setup(md, MOTOR_FAULT2_INT); + if (ret < 0) + goto failed_gpio; + md->irq_enabled = true; + /* enforce integrity check to handle fault state */ + moto_drv8424_cmd_push(md, CMD_INTEGRITY_CHK, 0); + } else { + /* motor can work, but have no fault irq */ + dev_info(dev, "Device has no fault irq\n"); + } + /* get ready for initial position detection */ + RESET_SENSOR_DATA; + POSITION_DETECT_INIT(POS_COMPACT, STATUS_STOPPED_COMPACT); + dev_info(dev, "Success init device; running position detection...\n"); - wake_up_process(md->motor_task); - - if(!set_pinctrl_state(md, INT_DEFAULT)) { - ret = moto_drv8424_irq_setup(md, MOTOR_FAULT1_INT); - if (ret < 0) - goto failed_gpio; - - ret = moto_drv8424_irq_setup(md, MOTOR_FAULT2_INT); - if (ret < 0) - goto failed_gpio; - - md->irq_enabled = true; - /* enforce integrity check to handle fault state */ - moto_drv8424_cmd_push(md, CMD_INTEGRITY_CHK, 0); - } else { - /* motor can work, but have no fault irq */ - dev_info(dev, "Device has no fault irq\n"); - } - - /* get ready for initial position detection */ - RESET_SENSOR_DATA; - POSITION_DETECT_INIT(POS_COMPACT, STATUS_STOPPED_COMPACT); - - dev_info(dev, "Success init device; running position detection...\n"); - return 0; + return 0; failed_gpio: - while(i >= MOTOR_POWER_EN) { + while (i >= MOTOR_POWER_EN) { if (gpio_is_valid(md->mc.ptable[i])) devm_gpio_free(dev, md->mc.ptable[i]); i--; - } - sysfs_remove_group(&md->dev->kobj, &status_attribute_group); + } + sysfs_remove_group(&md->dev->kobj, &status_attribute_group); failed_device: - device_destroy(md->drv8424_class, MKDEV(0, 0)); + device_destroy(md->drv8424_class, MKDEV(0, 0)); failed_sys_dev: - class_destroy(md->drv8424_class); + class_destroy(md->drv8424_class); failed_sys: - kthread_stop(md->motor_task); + kthread_stop(md->motor_task); failed_work: - destroy_workqueue(md->motor_wq); + destroy_workqueue(md->motor_wq); failed_kfifo: kfifo_free(&md->cmd_pipe); failed_mem: - kfree(md); + kfree(md); - return ret; + return ret; } static int moto_drv8424_remove(struct platform_device *pdev) { - motor_device* md = (motor_device*)platform_get_drvdata(pdev); - - if (md->irq_enabled) { - if (gpio_is_valid(md->mc.ptable[MOTOR_FAULT1_INT])) - disable_irq(gpio_to_irq(md->mc.ptable[MOTOR_FAULT1_INT])); - if (gpio_is_valid(md->mc.ptable[MOTOR_FAULT2_INT])) - disable_irq(gpio_to_irq(md->mc.ptable[MOTOR_FAULT2_INT])); - } - moto_drv8424_set_power(md, 0); - moto_drv8424_set_regulator_power(md, false); - devm_regulator_put(md->mc.vdd); - device_destroy(md->drv8424_class, MKDEV(0, 0)); - class_destroy(md->drv8424_class); - sysfs_remove_group(&md->dev->kobj, &status_attribute_group); - disable_motor(md->dev); - kthread_stop(md->motor_task); - destroy_workqueue(md->motor_wq); + motor_device* md = (motor_device*)platform_get_drvdata(pdev); + if (md->irq_enabled) { + if (gpio_is_valid(md->mc.ptable[MOTOR_FAULT1_INT])) + disable_irq(gpio_to_irq(md->mc.ptable[MOTOR_FAULT1_INT])); + if (gpio_is_valid(md->mc.ptable[MOTOR_FAULT2_INT])) + disable_irq(gpio_to_irq(md->mc.ptable[MOTOR_FAULT2_INT])); + } + moto_drv8424_set_power(md, 0); + moto_drv8424_set_regulator_power(md, false); + devm_regulator_put(md->mc.vdd); + device_destroy(md->drv8424_class, MKDEV(0, 0)); + class_destroy(md->drv8424_class); + sysfs_remove_group(&md->dev->kobj, &status_attribute_group); + disable_motor(md->dev); + kthread_stop(md->motor_task); + destroy_workqueue(md->motor_wq); kfifo_free(&md->cmd_pipe); - kfree(md); + kfree(md); - return 0; + return 0; } void moto_drv8424_platform_shutdown(struct platform_device *pdev) { - motor_device* md = (motor_device*)platform_get_drvdata(pdev); + motor_device* md = (motor_device*)platform_get_drvdata(pdev); int curpos = atomic_read(&md->position); - if (curpos != POS_COMPACT) { + if (curpos != POS_COMPACT) { md->power_en = 0; motor_set_motion_params(md, curpos, POS_COMPACT); atomic_set(&md->destination, POS_COMPACT); moto_drv8424_set_enable_with_power(md, true); - } - msleep(timeoutMs[md->regime]); + } + msleep(timeoutMs[md->regime]); } static const struct of_device_id moto_drv8424_match_table[] = {