From bedbef8ad3e75a304af6361af235302bcc61d06b Mon Sep 17 00:00:00 2001
From: hc <hc@nodka.com>
Date: Tue, 14 May 2024 06:39:01 +0000
Subject: [PATCH] 修改内核路径
---
kernel/drivers/iio/trigger/stm32-timer-trigger.c | 250 +++++++++++++++++++++++++++----------------------
1 files changed, 139 insertions(+), 111 deletions(-)
diff --git a/kernel/drivers/iio/trigger/stm32-timer-trigger.c b/kernel/drivers/iio/trigger/stm32-timer-trigger.c
index e7acb94..e38671b 100644
--- a/kernel/drivers/iio/trigger/stm32-timer-trigger.c
+++ b/kernel/drivers/iio/trigger/stm32-timer-trigger.c
@@ -75,14 +75,27 @@
{ }, /* timer 17 */
};
+struct stm32_timer_trigger_regs {
+ u32 cr1;
+ u32 cr2;
+ u32 psc;
+ u32 arr;
+ u32 cnt;
+ u32 smcr;
+};
+
struct stm32_timer_trigger {
struct device *dev;
struct regmap *regmap;
struct clk *clk;
+ bool enabled;
u32 max_arr;
const void *triggers;
const void *valids;
bool has_trgo2;
+ struct mutex lock; /* concurrent sysfs configuration */
+ struct list_head tr_list;
+ struct stm32_timer_trigger_regs bak;
};
struct stm32_timer_trigger_cfg {
@@ -106,7 +119,7 @@
{
unsigned long long prd, div;
int prescaler = 0;
- u32 ccer, cr1;
+ u32 ccer;
/* Period and prescaler values depends of clock rate */
div = (unsigned long long)clk_get_rate(priv->clk);
@@ -136,9 +149,11 @@
if (ccer & TIM_CCER_CCXE)
return -EBUSY;
- regmap_read(priv->regmap, TIM_CR1, &cr1);
- if (!(cr1 & TIM_CR1_CEN))
+ mutex_lock(&priv->lock);
+ if (!priv->enabled) {
+ priv->enabled = true;
clk_enable(priv->clk);
+ }
regmap_write(priv->regmap, TIM_PSC, prescaler);
regmap_write(priv->regmap, TIM_ARR, prd - 1);
@@ -157,6 +172,7 @@
/* Enable controller */
regmap_update_bits(priv->regmap, TIM_CR1, TIM_CR1_CEN, TIM_CR1_CEN);
+ mutex_unlock(&priv->lock);
return 0;
}
@@ -164,16 +180,13 @@
static void stm32_timer_stop(struct stm32_timer_trigger *priv,
struct iio_trigger *trig)
{
- u32 ccer, cr1;
+ u32 ccer;
regmap_read(priv->regmap, TIM_CCER, &ccer);
if (ccer & TIM_CCER_CCXE)
return;
- regmap_read(priv->regmap, TIM_CR1, &cr1);
- if (cr1 & TIM_CR1_CEN)
- clk_disable(priv->clk);
-
+ mutex_lock(&priv->lock);
/* Stop timer */
regmap_update_bits(priv->regmap, TIM_CR1, TIM_CR1_ARPE, 0);
regmap_update_bits(priv->regmap, TIM_CR1, TIM_CR1_CEN, 0);
@@ -188,6 +201,12 @@
/* Make sure that registers are updated */
regmap_update_bits(priv->regmap, TIM_EGR, TIM_EGR_UG, TIM_EGR_UG);
+
+ if (priv->enabled) {
+ priv->enabled = false;
+ clk_disable(priv->clk);
+ }
+ mutex_unlock(&priv->lock);
}
static ssize_t stm32_tt_store_frequency(struct device *dev,
@@ -302,11 +321,15 @@
for (i = 0; i <= master_mode_max; i++) {
if (!strncmp(master_mode_table[i], buf,
strlen(master_mode_table[i]))) {
+ mutex_lock(&priv->lock);
+ if (!priv->enabled) {
+ /* Clock should be enabled first */
+ priv->enabled = true;
+ clk_enable(priv->clk);
+ }
regmap_update_bits(priv->regmap, TIM_CR2, mask,
i << shift);
- /* Make sure that registers are updated */
- regmap_update_bits(priv->regmap, TIM_EGR,
- TIM_EGR_UG, TIM_EGR_UG);
+ mutex_unlock(&priv->lock);
return len;
}
}
@@ -364,10 +387,20 @@
static const struct iio_trigger_ops timer_trigger_ops = {
};
-static int stm32_setup_iio_triggers(struct stm32_timer_trigger *priv)
+static void stm32_unregister_iio_triggers(struct stm32_timer_trigger *priv)
+{
+ struct iio_trigger *tr;
+
+ list_for_each_entry(tr, &priv->tr_list, alloc_list)
+ iio_trigger_unregister(tr);
+}
+
+static int stm32_register_iio_triggers(struct stm32_timer_trigger *priv)
{
int ret;
const char * const *cur = priv->triggers;
+
+ INIT_LIST_HEAD(&priv->tr_list);
while (cur && *cur) {
struct iio_trigger *trig;
@@ -395,9 +428,13 @@
iio_trigger_set_drvdata(trig, priv);
- ret = devm_iio_trigger_register(priv->dev, trig);
- if (ret)
+ ret = iio_trigger_register(trig);
+ if (ret) {
+ stm32_unregister_iio_triggers(priv);
return ret;
+ }
+
+ list_add_tail(&trig->alloc_list, &priv->tr_list);
cur++;
}
@@ -444,7 +481,6 @@
int val, int val2, long mask)
{
struct stm32_timer_trigger *priv = iio_priv(indio_dev);
- u32 dat;
switch (mask) {
case IIO_CHAN_INFO_RAW:
@@ -455,19 +491,23 @@
return -EINVAL;
case IIO_CHAN_INFO_ENABLE:
+ mutex_lock(&priv->lock);
if (val) {
- regmap_read(priv->regmap, TIM_CR1, &dat);
- if (!(dat & TIM_CR1_CEN))
+ if (!priv->enabled) {
+ priv->enabled = true;
clk_enable(priv->clk);
+ }
regmap_update_bits(priv->regmap, TIM_CR1, TIM_CR1_CEN,
TIM_CR1_CEN);
} else {
- regmap_read(priv->regmap, TIM_CR1, &dat);
regmap_update_bits(priv->regmap, TIM_CR1, TIM_CR1_CEN,
0);
- if (dat & TIM_CR1_CEN)
+ if (priv->enabled) {
+ priv->enabled = false;
clk_disable(priv->clk);
+ }
}
+ mutex_unlock(&priv->lock);
return 0;
}
@@ -563,7 +603,6 @@
{
struct stm32_timer_trigger *priv = iio_priv(indio_dev);
int sms = stm32_enable_mode2sms(mode);
- u32 val;
if (sms < 0)
return sms;
@@ -571,11 +610,12 @@
* Triggered mode sets CEN bit automatically by hardware. So, first
* enable counter clock, so it can use it. Keeps it in sync with CEN.
*/
- if (sms == 6) {
- regmap_read(priv->regmap, TIM_CR1, &val);
- if (!(val & TIM_CR1_CEN))
- clk_enable(priv->clk);
+ mutex_lock(&priv->lock);
+ if (sms == 6 && !priv->enabled) {
+ clk_enable(priv->clk);
+ priv->enabled = true;
}
+ mutex_unlock(&priv->lock);
regmap_update_bits(priv->regmap, TIM_SMCR, TIM_SMCR_SMS, sms);
@@ -613,86 +653,6 @@
.num_items = ARRAY_SIZE(stm32_enable_modes),
.set = stm32_set_enable_mode,
.get = stm32_get_enable_mode
-};
-
-static const char *const stm32_quadrature_modes[] = {
- "channel_A",
- "channel_B",
- "quadrature",
-};
-
-static int stm32_set_quadrature_mode(struct iio_dev *indio_dev,
- const struct iio_chan_spec *chan,
- unsigned int mode)
-{
- struct stm32_timer_trigger *priv = iio_priv(indio_dev);
-
- regmap_update_bits(priv->regmap, TIM_SMCR, TIM_SMCR_SMS, mode + 1);
-
- return 0;
-}
-
-static int stm32_get_quadrature_mode(struct iio_dev *indio_dev,
- const struct iio_chan_spec *chan)
-{
- struct stm32_timer_trigger *priv = iio_priv(indio_dev);
- u32 smcr;
- int mode;
-
- regmap_read(priv->regmap, TIM_SMCR, &smcr);
- mode = (smcr & TIM_SMCR_SMS) - 1;
- if ((mode < 0) || (mode > ARRAY_SIZE(stm32_quadrature_modes)))
- return -EINVAL;
-
- return mode;
-}
-
-static const struct iio_enum stm32_quadrature_mode_enum = {
- .items = stm32_quadrature_modes,
- .num_items = ARRAY_SIZE(stm32_quadrature_modes),
- .set = stm32_set_quadrature_mode,
- .get = stm32_get_quadrature_mode
-};
-
-static const char *const stm32_count_direction_states[] = {
- "up",
- "down"
-};
-
-static int stm32_set_count_direction(struct iio_dev *indio_dev,
- const struct iio_chan_spec *chan,
- unsigned int dir)
-{
- struct stm32_timer_trigger *priv = iio_priv(indio_dev);
- u32 val;
- int mode;
-
- /* In encoder mode, direction is RO (given by TI1/TI2 signals) */
- regmap_read(priv->regmap, TIM_SMCR, &val);
- mode = (val & TIM_SMCR_SMS) - 1;
- if ((mode >= 0) || (mode < ARRAY_SIZE(stm32_quadrature_modes)))
- return -EBUSY;
-
- return regmap_update_bits(priv->regmap, TIM_CR1, TIM_CR1_DIR,
- dir ? TIM_CR1_DIR : 0);
-}
-
-static int stm32_get_count_direction(struct iio_dev *indio_dev,
- const struct iio_chan_spec *chan)
-{
- struct stm32_timer_trigger *priv = iio_priv(indio_dev);
- u32 cr1;
-
- regmap_read(priv->regmap, TIM_CR1, &cr1);
-
- return ((cr1 & TIM_CR1_DIR) ? 1 : 0);
-}
-
-static const struct iio_enum stm32_count_direction_enum = {
- .items = stm32_count_direction_states,
- .num_items = ARRAY_SIZE(stm32_count_direction_states),
- .set = stm32_set_count_direction,
- .get = stm32_get_count_direction
};
static ssize_t stm32_count_get_preset(struct iio_dev *indio_dev,
@@ -735,10 +695,6 @@
.read = stm32_count_get_preset,
.write = stm32_count_set_preset
},
- IIO_ENUM("count_direction", IIO_SEPARATE, &stm32_count_direction_enum),
- IIO_ENUM_AVAILABLE("count_direction", &stm32_count_direction_enum),
- IIO_ENUM("quadrature_mode", IIO_SEPARATE, &stm32_quadrature_mode_enum),
- IIO_ENUM_AVAILABLE("quadrature_mode", &stm32_quadrature_mode_enum),
IIO_ENUM("enable_mode", IIO_SEPARATE, &stm32_enable_mode_enum),
IIO_ENUM_AVAILABLE("enable_mode", &stm32_enable_mode_enum),
IIO_ENUM("trigger_mode", IIO_SEPARATE, &stm32_trigger_mode_enum),
@@ -767,12 +723,10 @@
return NULL;
indio_dev->name = dev_name(dev);
- indio_dev->dev.parent = dev;
indio_dev->info = &stm32_trigger_info;
indio_dev->modes = INDIO_HARDWARE_TRIGGERED;
indio_dev->num_channels = 1;
indio_dev->channels = &stm32_trigger_channel;
- indio_dev->dev.of_node = dev->of_node;
ret = devm_iio_device_register(dev, indio_dev);
if (ret)
@@ -843,8 +797,9 @@
priv->triggers = triggers_table[index];
priv->valids = cfg->valids_table[index];
stm32_timer_detect_trgo2(priv);
+ mutex_init(&priv->lock);
- ret = stm32_setup_iio_triggers(priv);
+ ret = stm32_register_iio_triggers(priv);
if (ret)
return ret;
@@ -852,6 +807,77 @@
return 0;
}
+
+static int stm32_timer_trigger_remove(struct platform_device *pdev)
+{
+ struct stm32_timer_trigger *priv = platform_get_drvdata(pdev);
+ u32 val;
+
+ /* Unregister triggers before everything can be safely turned off */
+ stm32_unregister_iio_triggers(priv);
+
+ /* Check if nobody else use the timer, then disable it */
+ regmap_read(priv->regmap, TIM_CCER, &val);
+ if (!(val & TIM_CCER_CCXE))
+ regmap_update_bits(priv->regmap, TIM_CR1, TIM_CR1_CEN, 0);
+
+ if (priv->enabled)
+ clk_disable(priv->clk);
+
+ return 0;
+}
+
+static int __maybe_unused stm32_timer_trigger_suspend(struct device *dev)
+{
+ struct stm32_timer_trigger *priv = dev_get_drvdata(dev);
+
+ /* Only take care of enabled timer: don't disturb other MFD child */
+ if (priv->enabled) {
+ /* Backup registers that may get lost in low power mode */
+ regmap_read(priv->regmap, TIM_CR1, &priv->bak.cr1);
+ regmap_read(priv->regmap, TIM_CR2, &priv->bak.cr2);
+ regmap_read(priv->regmap, TIM_PSC, &priv->bak.psc);
+ regmap_read(priv->regmap, TIM_ARR, &priv->bak.arr);
+ regmap_read(priv->regmap, TIM_CNT, &priv->bak.cnt);
+ regmap_read(priv->regmap, TIM_SMCR, &priv->bak.smcr);
+
+ /* Disable the timer */
+ regmap_update_bits(priv->regmap, TIM_CR1, TIM_CR1_CEN, 0);
+ clk_disable(priv->clk);
+ }
+
+ return 0;
+}
+
+static int __maybe_unused stm32_timer_trigger_resume(struct device *dev)
+{
+ struct stm32_timer_trigger *priv = dev_get_drvdata(dev);
+ int ret;
+
+ if (priv->enabled) {
+ ret = clk_enable(priv->clk);
+ if (ret)
+ return ret;
+
+ /* restore master/slave modes */
+ regmap_write(priv->regmap, TIM_SMCR, priv->bak.smcr);
+ regmap_write(priv->regmap, TIM_CR2, priv->bak.cr2);
+
+ /* restore sampling_frequency (trgo / trgo2 triggers) */
+ regmap_write(priv->regmap, TIM_PSC, priv->bak.psc);
+ regmap_write(priv->regmap, TIM_ARR, priv->bak.arr);
+ regmap_write(priv->regmap, TIM_CNT, priv->bak.cnt);
+
+ /* Also re-enables the timer */
+ regmap_write(priv->regmap, TIM_CR1, priv->bak.cr1);
+ }
+
+ return 0;
+}
+
+static SIMPLE_DEV_PM_OPS(stm32_timer_trigger_pm_ops,
+ stm32_timer_trigger_suspend,
+ stm32_timer_trigger_resume);
static const struct stm32_timer_trigger_cfg stm32_timer_trg_cfg = {
.valids_table = valids_table,
@@ -877,9 +903,11 @@
static struct platform_driver stm32_timer_trigger_driver = {
.probe = stm32_timer_trigger_probe,
+ .remove = stm32_timer_trigger_remove,
.driver = {
.name = "stm32-timer-trigger",
.of_match_table = stm32_trig_of_match,
+ .pm = &stm32_timer_trigger_pm_ops,
},
};
module_platform_driver(stm32_timer_trigger_driver);
--
Gitblit v1.6.2