@@ -449,9 +449,10 @@ static const struct regmap_config sdm845_llcc_bwmon_regmap_cfg = {
.cache_type = REGCACHE_RBTREE,
};
-static void bwmon_clear_counters(struct icc_bwmon *bwmon, bool clear_all)
+static int bwmon_clear_counters(struct icc_bwmon *bwmon, bool clear_all)
{
unsigned int val = BWMON_CLEAR_CLEAR;
+ int ret;
if (clear_all)
val |= BWMON_CLEAR_CLEAR_ALL;
@@ -463,14 +464,20 @@ static void bwmon_clear_counters(struct icc_bwmon *bwmon, bool clear_all)
* region. So, we need to make sure the counter clear is completed
* before we try to clear the IRQ or do any other counter operations.
*/
- regmap_field_force_write(bwmon->regs[F_CLEAR], val);
+ ret = regmap_field_force_write(bwmon->regs[F_CLEAR], val);
+ if (ret)
+ return ret;
+
if (bwmon->data->quirks & BWMON_NEEDS_FORCE_CLEAR)
- regmap_field_force_write(bwmon->regs[F_CLEAR], 0);
+ ret = regmap_field_force_write(bwmon->regs[F_CLEAR], 0);
+
+ return ret;
}
-static void bwmon_clear_irq(struct icc_bwmon *bwmon)
+static int bwmon_clear_irq(struct icc_bwmon *bwmon)
{
struct regmap_field *global_irq_clr;
+ int ret;
if (bwmon->data->global_regmap_fields)
global_irq_clr = bwmon->global_regs[F_GLOBAL_IRQ_CLEAR];
@@ -493,17 +500,27 @@ static void bwmon_clear_irq(struct icc_bwmon *bwmon)
* clearing here so that local writes don't happen before the
* interrupt is cleared.
*/
- regmap_field_force_write(bwmon->regs[F_IRQ_CLEAR], BWMON_IRQ_ENABLE_MASK);
- if (bwmon->data->quirks & BWMON_NEEDS_FORCE_CLEAR)
- regmap_field_force_write(bwmon->regs[F_IRQ_CLEAR], 0);
+ ret = regmap_field_force_write(bwmon->regs[F_IRQ_CLEAR], BWMON_IRQ_ENABLE_MASK);
+ if (ret)
+ return ret;
+
+ if (bwmon->data->quirks & BWMON_NEEDS_FORCE_CLEAR) {
+ ret = regmap_field_force_write(bwmon->regs[F_IRQ_CLEAR], 0);
+ if (ret)
+ return ret;
+ }
+
if (bwmon->data->quirks & BWMON_HAS_GLOBAL_IRQ)
- regmap_field_force_write(global_irq_clr,
- BWMON_V4_GLOBAL_IRQ_ENABLE_ENABLE);
+ ret = regmap_field_force_write(global_irq_clr,
+ BWMON_V4_GLOBAL_IRQ_ENABLE_ENABLE);
+
+ return ret;
}
-static void bwmon_disable(struct icc_bwmon *bwmon)
+static int bwmon_disable(struct icc_bwmon *bwmon)
{
struct regmap_field *global_irq_en;
+ int ret;
if (bwmon->data->global_regmap_fields)
global_irq_en = bwmon->global_regs[F_GLOBAL_IRQ_ENABLE];
@@ -511,20 +528,29 @@ static void bwmon_disable(struct icc_bwmon *bwmon)
global_irq_en = bwmon->regs[F_GLOBAL_IRQ_ENABLE];
/* Disable interrupts. Strict ordering, see bwmon_clear_irq(). */
- if (bwmon->data->quirks & BWMON_HAS_GLOBAL_IRQ)
- regmap_field_write(global_irq_en, 0x0);
- regmap_field_write(bwmon->regs[F_IRQ_ENABLE], 0x0);
+ if (bwmon->data->quirks & BWMON_HAS_GLOBAL_IRQ) {
+ ret = regmap_field_write(global_irq_en, 0x0);
+ if (ret)
+ return ret;
+ }
+
+ ret = regmap_field_write(bwmon->regs[F_IRQ_ENABLE], 0x0);
+ if (ret)
+ return ret;
/*
* Disable bwmon. Must happen before bwmon_clear_irq() to avoid spurious
* IRQ.
*/
- regmap_field_write(bwmon->regs[F_ENABLE], 0x0);
+ ret = regmap_field_write(bwmon->regs[F_ENABLE], 0x0);
+
+ return ret;
}
-static void bwmon_enable(struct icc_bwmon *bwmon, unsigned int irq_enable)
+static int bwmon_enable(struct icc_bwmon *bwmon, unsigned int irq_enable)
{
struct regmap_field *global_irq_en;
+ int ret;
if (bwmon->data->global_regmap_fields)
global_irq_en = bwmon->global_regs[F_GLOBAL_IRQ_ENABLE];
@@ -532,14 +558,21 @@ static void bwmon_enable(struct icc_bwmon *bwmon, unsigned int irq_enable)
global_irq_en = bwmon->regs[F_GLOBAL_IRQ_ENABLE];
/* Enable interrupts */
- if (bwmon->data->quirks & BWMON_HAS_GLOBAL_IRQ)
- regmap_field_write(global_irq_en,
- BWMON_V4_GLOBAL_IRQ_ENABLE_ENABLE);
+ if (bwmon->data->quirks & BWMON_HAS_GLOBAL_IRQ) {
+ ret = regmap_field_write(global_irq_en,
+ BWMON_V4_GLOBAL_IRQ_ENABLE_ENABLE);
+ if (ret)
+ return ret;
+ }
- regmap_field_write(bwmon->regs[F_IRQ_ENABLE], irq_enable);
+ ret = regmap_field_write(bwmon->regs[F_IRQ_ENABLE], irq_enable);
+ if (ret)
+ return ret;
/* Enable bwmon */
- regmap_field_write(bwmon->regs[F_ENABLE], BWMON_ENABLE_ENABLE);
+ ret = regmap_field_write(bwmon->regs[F_ENABLE], BWMON_ENABLE_ENABLE);
+
+ return ret;
}
static unsigned int bwmon_kbps_to_count(struct icc_bwmon *bwmon,
@@ -548,55 +581,97 @@ static unsigned int bwmon_kbps_to_count(struct icc_bwmon *bwmon,
return kbps / bwmon->data->count_unit_kb;
}
-static void bwmon_set_threshold(struct icc_bwmon *bwmon,
+static int bwmon_set_threshold(struct icc_bwmon *bwmon,
struct regmap_field *reg, unsigned int kbps)
{
unsigned int thres;
thres = mult_frac(bwmon_kbps_to_count(bwmon, kbps),
bwmon->data->sample_ms, MSEC_PER_SEC);
- regmap_field_write(reg, thres);
+ return regmap_field_write(reg, thres);
}
-static void bwmon_start(struct icc_bwmon *bwmon)
+static int bwmon_start(struct icc_bwmon *bwmon)
{
const struct icc_bwmon_data *data = bwmon->data;
+ int ret, window;
u32 bw_low = 0;
- int window;
/* No need to check for errors, as this must have succeeded before. */
dev_pm_opp_find_bw_ceil(bwmon->dev, &bw_low, 0);
- bwmon_clear_counters(bwmon, true);
+ ret = bwmon_clear_counters(bwmon, true);
+ if (ret)
+ return ret;
window = mult_frac(bwmon->data->sample_ms, HW_TIMER_HZ, MSEC_PER_SEC);
/* Maximum sampling window: 0xffffff for v4 and 0xfffff for v5 */
- regmap_field_write(bwmon->regs[F_SAMPLE_WINDOW], window);
-
- bwmon_set_threshold(bwmon, bwmon->regs[F_THRESHOLD_HIGH], bw_low);
- bwmon_set_threshold(bwmon, bwmon->regs[F_THRESHOLD_MED], bw_low);
- bwmon_set_threshold(bwmon, bwmon->regs[F_THRESHOLD_LOW], 0);
-
- regmap_field_write(bwmon->regs[F_THRESHOLD_COUNT_ZONE0],
- BWMON_THRESHOLD_COUNT_ZONE0_DEFAULT);
- regmap_field_write(bwmon->regs[F_THRESHOLD_COUNT_ZONE1],
- data->zone1_thres_count);
- regmap_field_write(bwmon->regs[F_THRESHOLD_COUNT_ZONE2],
- BWMON_THRESHOLD_COUNT_ZONE2_DEFAULT);
- regmap_field_write(bwmon->regs[F_THRESHOLD_COUNT_ZONE3],
- data->zone3_thres_count);
-
- regmap_field_write(bwmon->regs[F_ZONE_ACTIONS_ZONE0],
- BWMON_ZONE_ACTIONS_ZONE0);
- regmap_field_write(bwmon->regs[F_ZONE_ACTIONS_ZONE1],
- BWMON_ZONE_ACTIONS_ZONE1);
- regmap_field_write(bwmon->regs[F_ZONE_ACTIONS_ZONE2],
- BWMON_ZONE_ACTIONS_ZONE2);
- regmap_field_write(bwmon->regs[F_ZONE_ACTIONS_ZONE3],
- BWMON_ZONE_ACTIONS_ZONE3);
-
- bwmon_clear_irq(bwmon);
- bwmon_enable(bwmon, BWMON_IRQ_ENABLE_MASK);
+ ret = regmap_field_write(bwmon->regs[F_SAMPLE_WINDOW], window);
+ if (ret)
+ return ret;
+
+ /* Set up bandwidth thresholds */
+ ret = bwmon_set_threshold(bwmon, bwmon->regs[F_THRESHOLD_HIGH], bw_low);
+ if (ret)
+ return ret;
+
+ ret = bwmon_set_threshold(bwmon, bwmon->regs[F_THRESHOLD_MED], bw_low);
+ if (ret)
+ return ret;
+
+ ret = bwmon_set_threshold(bwmon, bwmon->regs[F_THRESHOLD_LOW], 0);
+ if (ret)
+ return ret;
+
+ /* Set up threshold counts */
+ ret = regmap_field_write(bwmon->regs[F_THRESHOLD_COUNT_ZONE0],
+ BWMON_THRESHOLD_COUNT_ZONE0_DEFAULT);
+ if (ret)
+ return ret;
+
+ ret = regmap_field_write(bwmon->regs[F_THRESHOLD_COUNT_ZONE1],
+ data->zone1_thres_count);
+ if (ret)
+ return ret;
+
+ ret = regmap_field_write(bwmon->regs[F_THRESHOLD_COUNT_ZONE2],
+ BWMON_THRESHOLD_COUNT_ZONE2_DEFAULT);
+ if (ret)
+ return ret;
+
+ ret = regmap_field_write(bwmon->regs[F_THRESHOLD_COUNT_ZONE3],
+ data->zone3_thres_count);
+ if (ret)
+ return ret;
+
+ /* Set up zone actions */
+ ret = regmap_field_write(bwmon->regs[F_ZONE_ACTIONS_ZONE0],
+ BWMON_ZONE_ACTIONS_ZONE0);
+ if (ret)
+ return ret;
+
+ ret = regmap_field_write(bwmon->regs[F_ZONE_ACTIONS_ZONE1],
+ BWMON_ZONE_ACTIONS_ZONE1);
+ if (ret)
+ return ret;
+
+ ret = regmap_field_write(bwmon->regs[F_ZONE_ACTIONS_ZONE2],
+ BWMON_ZONE_ACTIONS_ZONE2);
+ if (ret)
+ return ret;
+
+ ret = regmap_field_write(bwmon->regs[F_ZONE_ACTIONS_ZONE3],
+ BWMON_ZONE_ACTIONS_ZONE3);
+ if (ret)
+ return ret;
+
+ /* Clear any previous interrupts and get the stone rolling! */
+ ret = bwmon_clear_irq(bwmon);
+ if (ret)
+ return ret;
+
+
+ return bwmon_enable(bwmon, BWMON_IRQ_ENABLE_MASK);
}
static irqreturn_t bwmon_intr(int irq, void *dev_id)
@@ -622,7 +697,8 @@ static irqreturn_t bwmon_intr(int irq, void *dev_id)
return IRQ_NONE;
}
- bwmon_disable(bwmon);
+ if (bwmon_disable(bwmon))
+ return IRQ_NONE;
zone = get_bitmask_order(status) - 1;
/*
@@ -671,13 +747,20 @@ static irqreturn_t bwmon_intr_thread(int irq, void *dev_id)
else
irq_enable = BWMON_IRQ_ENABLE_MASK;
- bwmon_set_threshold(bwmon, bwmon->regs[F_THRESHOLD_HIGH],
- up_kbps);
- bwmon_set_threshold(bwmon, bwmon->regs[F_THRESHOLD_MED],
- down_kbps);
- bwmon_clear_counters(bwmon, false);
- bwmon_clear_irq(bwmon);
- bwmon_enable(bwmon, irq_enable);
+ if (bwmon_set_threshold(bwmon, bwmon->regs[F_THRESHOLD_HIGH], up_kbps))
+ return IRQ_NONE;
+
+ if (bwmon_set_threshold(bwmon, bwmon->regs[F_THRESHOLD_MED], down_kbps))
+ return IRQ_NONE;
+
+ if (bwmon_clear_counters(bwmon, false))
+ return IRQ_NONE;
+
+ if (bwmon_clear_irq(bwmon))
+ return IRQ_NONE;
+
+ if (bwmon_enable(bwmon, irq_enable))
+ return IRQ_NONE;
if (bwmon->target_kbps == bwmon->current_kbps)
goto out;
@@ -780,7 +863,10 @@ static int bwmon_probe(struct platform_device *pdev)
bwmon->dev = dev;
- bwmon_disable(bwmon);
+ ret = bwmon_disable(bwmon);
+ if (ret)
+ return dev_err_probe(dev, ret, "failed to disable BWMON\n");
+
ret = devm_request_threaded_irq(dev, bwmon->irq, bwmon_intr,
bwmon_intr_thread,
IRQF_ONESHOT, dev_name(dev), bwmon);
@@ -788,7 +874,10 @@ static int bwmon_probe(struct platform_device *pdev)
return dev_err_probe(dev, ret, "failed to request IRQ\n");
platform_set_drvdata(pdev, bwmon);
- bwmon_start(bwmon);
+
+ ret = bwmon_start(bwmon);
+ if (ret)
+ return dev_err_probe(dev, ret, "failed to start BWMON\n");
return 0;
}