1 files changed, 280 insertions, 126 deletions

diff --git a/drivers/iio/industrialio-gts-helper.c b/drivers/iio/industrialio-gts-helper.c
index 7653261d2dc2..f35c36fd4a55 100644
--- a/drivers/iio/industrialio-gts-helper.c
+++ b/drivers/iio/industrialio-gts-helper.c
@@ -34,24 +34,11 @@
 static int iio_gts_get_gain(const u64 max, const u64 scale)
 {
 	u64 full = max;
-	int tmp = 1;
 
 	if (scale > full || !scale)
 		return -EINVAL;
 
-	if (U64_MAX - full < scale) {
-		/* Risk of overflow */
-		if (full - scale < scale)
-			return 1;
-
-		full -= scale;
-		tmp++;
-	}
-
-	while (full > scale * (u64)tmp)
-		tmp++;
-
-	return tmp;
+	return div64_u64(full, scale);
 }
 
 /**
@@ -147,7 +134,7 @@ int iio_gts_total_gain_to_scale(struct iio_gts *gts, int total_gain,
 
 	return iio_gts_delinearize(tmp, NANO, scale_int, scale_nano);
 }
-EXPORT_SYMBOL_NS_GPL(iio_gts_total_gain_to_scale, IIO_GTS_HELPER);
+EXPORT_SYMBOL_NS_GPL(iio_gts_total_gain_to_scale, "IIO_GTS_HELPER");
 
 /**
  * iio_gts_purge_avail_scale_table - free-up the available scale tables
@@ -173,16 +160,123 @@ static void iio_gts_purge_avail_scale_table(struct iio_gts *gts)
 	gts->num_avail_all_scales = 0;
 }
 
+static int scale_eq(int *sc1, int *sc2)
+{
+	return sc1[0] == sc2[0] && sc1[1] == sc2[1];
+}
+
+static int scale_smaller(int *sc1, int *sc2)
+{
+	if (sc1[0] != sc2[0])
+		return sc1[0] < sc2[0];
+
+	/* If integer parts are equal, fixp parts */
+	return sc1[1] < sc2[1];
+}
+
+/*
+ * Do a single table listing all the unique scales that any combination of
+ * supported gains and times can provide.
+ */
+static int do_combined_scaletable(struct iio_gts *gts,
+				  size_t all_scales_tbl_bytes)
+{
+	int t_idx, i, new_idx;
+	int **scales = gts->per_time_avail_scale_tables;
+	int *all_scales = kcalloc(gts->num_itime, all_scales_tbl_bytes,
+				  GFP_KERNEL);
+
+	if (!all_scales)
+		return -ENOMEM;
+	/*
+	 * Create table containing all of the supported scales by looping
+	 * through all of the per-time scales and copying the unique scales
+	 * into one sorted table.
+	 *
+	 * We assume all the gains for same integration time were unique.
+	 * It is likely the first time table had greatest time multiplier as
+	 * the times are in the order of preference and greater times are
+	 * usually preferred. Hence we start from the last table which is likely
+	 * to have the smallest total gains.
+	 */
+	t_idx = gts->num_itime - 1;
+	memcpy(all_scales, scales[t_idx], all_scales_tbl_bytes);
+	new_idx = gts->num_hwgain * 2;
+
+	while (t_idx-- > 0) {
+		for (i = 0; i < gts->num_hwgain ; i++) {
+			int *candidate = &scales[t_idx][i * 2];
+			int chk;
+
+			if (scale_smaller(candidate, &all_scales[new_idx - 2])) {
+				all_scales[new_idx] = candidate[0];
+				all_scales[new_idx + 1] = candidate[1];
+				new_idx += 2;
+
+				continue;
+			}
+			for (chk = 0; chk < new_idx; chk += 2)
+				if (!scale_smaller(candidate, &all_scales[chk]))
+					break;
+
+			if (scale_eq(candidate, &all_scales[chk]))
+				continue;
+
+			memmove(&all_scales[chk + 2], &all_scales[chk],
+				(new_idx - chk) * sizeof(int));
+			all_scales[chk] = candidate[0];
+			all_scales[chk + 1] = candidate[1];
+			new_idx += 2;
+		}
+	}
+
+	gts->num_avail_all_scales = new_idx / 2;
+	gts->avail_all_scales_table = all_scales;
+
+	return 0;
+}
+
+static void iio_gts_free_int_table_array(int **arr, int num_tables)
+{
+	int i;
+
+	for (i = 0; i < num_tables; i++)
+		kfree(arr[i]);
+
+	kfree(arr);
+}
+
+static int iio_gts_alloc_int_table_array(int ***arr, int num_tables, int num_table_items)
+{
+	int i, **tmp;
+
+	tmp = kcalloc(num_tables, sizeof(**arr), GFP_KERNEL);
+	if (!tmp)
+		return -ENOMEM;
+
+	for (i = 0; i < num_tables; i++) {
+		tmp[i] = kcalloc(num_table_items, sizeof(int), GFP_KERNEL);
+		if (!tmp[i])
+			goto err_free;
+	}
+
+	*arr = tmp;
+
+	return 0;
+err_free:
+	iio_gts_free_int_table_array(tmp, i);
+
+	return -ENOMEM;
+}
+
 static int iio_gts_gain_cmp(const void *a, const void *b)
 {
 	return *(int *)a - *(int *)b;
 }
 
-static int gain_to_scaletables(struct iio_gts *gts, int **gains, int **scales)
+static int fill_and_sort_scaletables(struct iio_gts *gts, int **gains, int **scales)
 {
-	int ret, i, j, new_idx, time_idx;
-	int *all_gains;
-	size_t gain_bytes;
+	int i, j, ret;
 
 	for (i = 0; i < gts->num_itime; i++) {
 		/*
@@ -202,71 +296,69 @@ static int gain_to_scaletables(struct iio_gts *gts, int **gains, int **scales)
 		}
 	}
 
-	gain_bytes = array_size(gts->num_hwgain, sizeof(int));
-	all_gains = kcalloc(gts->num_itime, gain_bytes, GFP_KERNEL);
-	if (!all_gains)
-		return -ENOMEM;
+	return 0;
+}
+
+static void compute_per_time_gains(struct iio_gts *gts, int **gains)
+{
+	int i, j;
+
+	for (i = 0; i < gts->num_itime; i++) {
+		for (j = 0; j < gts->num_hwgain; j++)
+			gains[i][j] = gts->hwgain_table[j].gain *
+				      gts->itime_table[i].mul;
+	}
+}
+
+static int compute_per_time_tables(struct iio_gts *gts, int **scales)
+{
+	int **per_time_gains;
+	int ret;
 
 	/*
-	 * We assume all the gains for same integration time were unique.
-	 * It is likely the first time table had greatest time multiplier as
-	 * the times are in the order of preference and greater times are
-	 * usually preferred. Hence we start from the last table which is likely
-	 * to have the smallest total gains.
+	 * Create a temporary array of the 'total gains' for each integration
+	 * time.
 	 */
-	time_idx = gts->num_itime - 1;
-	memcpy(all_gains, gains[time_idx], gain_bytes);
-	new_idx = gts->num_hwgain;
+	ret = iio_gts_alloc_int_table_array(&per_time_gains, gts->num_itime,
+					    gts->num_hwgain);
+	if (ret)
+		return ret;
 
-	while (time_idx--) {
-		for (j = 0; j < gts->num_hwgain; j++) {
-			int candidate = gains[time_idx][j];
-			int chk;
+	compute_per_time_gains(gts, per_time_gains);
 
-			if (candidate > all_gains[new_idx - 1]) {
-				all_gains[new_idx] = candidate;
-				new_idx++;
+	/* Convert the gains to scales and populate the scale tables */
+	ret = fill_and_sort_scaletables(gts, per_time_gains, scales);
 
-				continue;
-			}
-			for (chk = 0; chk < new_idx; chk++)
-				if (candidate <= all_gains[chk])
-					break;
+	iio_gts_free_int_table_array(per_time_gains, gts->num_itime);
 
-			if (candidate == all_gains[chk])
-				continue;
+	return ret;
+}
 
-			memmove(&all_gains[chk + 1], &all_gains[chk],
-				(new_idx - chk) * sizeof(int));
-			all_gains[chk] = candidate;
-			new_idx++;
-		}
-	}
+/*
+ * Create a table of supported scales for each supported integration time.
+ * This can be used as available_scales by drivers which don't allow scale
+ * setting to change the integration time to display correct set of scales
+ * depending on the used integration time.
+ */
+static int **create_per_time_scales(struct iio_gts *gts)
+{
+	int **per_time_scales, ret;
 
-	gts->avail_all_scales_table = kcalloc(new_idx, 2 * sizeof(int),
-					      GFP_KERNEL);
-	if (!gts->avail_all_scales_table) {
-		ret = -ENOMEM;
-		goto free_out;
-	}
-	gts->num_avail_all_scales = new_idx;
+	ret = iio_gts_alloc_int_table_array(&per_time_scales, gts->num_itime,
+					    gts->num_hwgain * 2);
+	if (ret)
+		return ERR_PTR(ret);
 
-	for (i = 0; i < gts->num_avail_all_scales; i++) {
-		ret = iio_gts_total_gain_to_scale(gts, all_gains[i],
-					&gts->avail_all_scales_table[i * 2],
-					&gts->avail_all_scales_table[i * 2 + 1]);
+	ret = compute_per_time_tables(gts, per_time_scales);
+	if (ret)
+		goto err_out;
 
-		if (ret) {
-			kfree(gts->avail_all_scales_table);
-			gts->num_avail_all_scales = 0;
-			goto free_out;
-		}
-	}
+	return per_time_scales;
 
-free_out:
-	kfree(all_gains);
+err_out:
+	iio_gts_free_int_table_array(per_time_scales, gts->num_itime);
 
-	return ret;
+	return ERR_PTR(ret);
 }
 
 /**
@@ -288,53 +380,26 @@ free_out:
  */
 static int iio_gts_build_avail_scale_table(struct iio_gts *gts)
 {
-	int **per_time_gains, **per_time_scales, i, j, ret = -ENOMEM;
+	int ret, all_scales_tbl_bytes;
+	int **per_time_scales;
 
-	per_time_gains = kcalloc(gts->num_itime, sizeof(*per_time_gains), GFP_KERNEL);
-	if (!per_time_gains)
-		return ret;
-
-	per_time_scales = kcalloc(gts->num_itime, sizeof(*per_time_scales), GFP_KERNEL);
-	if (!per_time_scales)
-		goto free_gains;
-
-	for (i = 0; i < gts->num_itime; i++) {
-		per_time_scales[i] = kcalloc(gts->num_hwgain, 2 * sizeof(int),
-					     GFP_KERNEL);
-		if (!per_time_scales[i])
-			goto err_free_out;
-
-		per_time_gains[i] = kcalloc(gts->num_hwgain, sizeof(int),
-					    GFP_KERNEL);
-		if (!per_time_gains[i]) {
-			kfree(per_time_scales[i]);
-			goto err_free_out;
-		}
-
-		for (j = 0; j < gts->num_hwgain; j++)
-			per_time_gains[i][j] = gts->hwgain_table[j].gain *
-					       gts->itime_table[i].mul;
-	}
+	if (unlikely(check_mul_overflow(gts->num_hwgain, 2 * sizeof(int),
+					&all_scales_tbl_bytes)))
+		return -EOVERFLOW;
 
-	ret = gain_to_scaletables(gts, per_time_gains, per_time_scales);
-	if (ret)
-		goto err_free_out;
+	per_time_scales = create_per_time_scales(gts);
+	if (IS_ERR(per_time_scales))
+		return PTR_ERR(per_time_scales);
 
-	kfree(per_time_gains);
 	gts->per_time_avail_scale_tables = per_time_scales;
 
-	return 0;
-
-err_free_out:
-	for (i--; i; i--) {
-		kfree(per_time_scales[i]);
-		kfree(per_time_gains[i]);
+	ret = do_combined_scaletable(gts, all_scales_tbl_bytes);
+	if (ret) {
+		iio_gts_free_int_table_array(per_time_scales, gts->num_itime);
+		return ret;
 	}
-	kfree(per_time_scales);
-free_gains:
-	kfree(per_time_gains);
 
-	return ret;
+	return 0;
 }
 
 static void iio_gts_us_to_int_micro(int *time_us, int *int_micro_times,
@@ -375,17 +440,20 @@ static int iio_gts_build_avail_time_table(struct iio_gts *gts)
 	for (i = gts->num_itime - 1; i >= 0; i--) {
 		int new = gts->itime_table[i].time_us;
 
-		if (times[idx] < new) {
+		if (idx == 0 || times[idx - 1] < new) {
 			times[idx++] = new;
 			continue;
 		}
 
-		for (j = 0; j <= idx; j++) {
+		for (j = 0; j < idx; j++) {
+			if (times[j] == new)
+				break;
 			if (times[j] > new) {
 				memmove(&times[j + 1], &times[j],
 					(idx - j) * sizeof(int));
 				times[j] = new;
 				idx++;
+				break;
 			}
 		}
 	}
@@ -630,7 +698,7 @@ int devm_iio_init_iio_gts(struct device *dev, int max_scale_int, int max_scale_n
 
 	return devm_iio_gts_build_avail_tables(dev, gts);
 }
-EXPORT_SYMBOL_NS_GPL(devm_iio_init_iio_gts, IIO_GTS_HELPER);
+EXPORT_SYMBOL_NS_GPL(devm_iio_init_iio_gts, "IIO_GTS_HELPER");
 
 /**
  * iio_gts_all_avail_scales - helper for listing all available scales
@@ -653,7 +721,7 @@ int iio_gts_all_avail_scales(struct iio_gts *gts, const int **vals, int *type,
 
 	return IIO_AVAIL_LIST;
 }
-EXPORT_SYMBOL_NS_GPL(iio_gts_all_avail_scales, IIO_GTS_HELPER);
+EXPORT_SYMBOL_NS_GPL(iio_gts_all_avail_scales, "IIO_GTS_HELPER");
 
 /**
  * iio_gts_avail_scales_for_time - list scales for integration time
@@ -687,7 +755,7 @@ int iio_gts_avail_scales_for_time(struct iio_gts *gts, int time,
 
 	return IIO_AVAIL_LIST;
 }
-EXPORT_SYMBOL_NS_GPL(iio_gts_avail_scales_for_time, IIO_GTS_HELPER);
+EXPORT_SYMBOL_NS_GPL(iio_gts_avail_scales_for_time, "IIO_GTS_HELPER");
 
 /**
  * iio_gts_avail_times - helper for listing available integration times
@@ -710,7 +778,7 @@ int iio_gts_avail_times(struct iio_gts *gts,  const int **vals, int *type,
 
 	return IIO_AVAIL_LIST;
 }
-EXPORT_SYMBOL_NS_GPL(iio_gts_avail_times, IIO_GTS_HELPER);
+EXPORT_SYMBOL_NS_GPL(iio_gts_avail_times, "IIO_GTS_HELPER");
 
 /**
  * iio_gts_find_sel_by_gain - find selector corresponding to a HW-gain
@@ -730,7 +798,7 @@ int iio_gts_find_sel_by_gain(struct iio_gts *gts, int gain)
 
 	return -EINVAL;
 }
-EXPORT_SYMBOL_NS_GPL(iio_gts_find_sel_by_gain, IIO_GTS_HELPER);
+EXPORT_SYMBOL_NS_GPL(iio_gts_find_sel_by_gain, "IIO_GTS_HELPER");
 
 /**
  * iio_gts_find_gain_by_sel - find HW-gain corresponding to a selector
@@ -750,7 +818,7 @@ int iio_gts_find_gain_by_sel(struct iio_gts *gts, int sel)
 
 	return -EINVAL;
 }
-EXPORT_SYMBOL_NS_GPL(iio_gts_find_gain_by_sel, IIO_GTS_HELPER);
+EXPORT_SYMBOL_NS_GPL(iio_gts_find_gain_by_sel, "IIO_GTS_HELPER");
 
 /**
  * iio_gts_get_min_gain - find smallest valid HW-gain
@@ -773,7 +841,7 @@ int iio_gts_get_min_gain(struct iio_gts *gts)
 
 	return min;
 }
-EXPORT_SYMBOL_NS_GPL(iio_gts_get_min_gain, IIO_GTS_HELPER);
+EXPORT_SYMBOL_NS_GPL(iio_gts_get_min_gain, "IIO_GTS_HELPER");
 
 /**
  * iio_find_closest_gain_low - Find the closest lower matching gain
@@ -834,7 +902,7 @@ int iio_find_closest_gain_low(struct iio_gts *gts, int gain, bool *in_range)
 
 	return gts->hwgain_table[best].gain;
 }
-EXPORT_SYMBOL_NS_GPL(iio_find_closest_gain_low, IIO_GTS_HELPER);
+EXPORT_SYMBOL_NS_GPL(iio_find_closest_gain_low, "IIO_GTS_HELPER");
 
 static int iio_gts_get_int_time_gain_multiplier_by_sel(struct iio_gts *gts,
 						       int sel)
@@ -921,9 +989,52 @@ int iio_gts_find_gain_sel_for_scale_using_time(struct iio_gts *gts, int time_sel
 
 	return 0;
 }
-EXPORT_SYMBOL_NS_GPL(iio_gts_find_gain_sel_for_scale_using_time, IIO_GTS_HELPER);
+EXPORT_SYMBOL_NS_GPL(iio_gts_find_gain_sel_for_scale_using_time, "IIO_GTS_HELPER");
+
+/**
+ * iio_gts_find_gain_time_sel_for_scale - Fetch gain and time selectors for scale
+ * @gts:	Gain time scale descriptor
+ * @scale_int:	Integral part of the scale (typically val1)
+ * @scale_nano:	Fractional part of the scale (nano or ppb)
+ * @gain_sel:	Pointer to value where gain selector is stored.
+ * @time_sel:	Pointer to value where time selector is stored.
+ *
+ * Wrapper around iio_gts_find_gain_for_scale_using_time() to fetch the
+ * gain and time selectors for a given scale.
+ *
+ * Return: 0 on success and -EINVAL on error.
+ */
+int iio_gts_find_gain_time_sel_for_scale(struct iio_gts *gts, int scale_int,
+					 int scale_nano, int *gain_sel,
+					 int *time_sel)
+{
+	int i, ret;
+
+	for (i = 0; i < gts->num_itime; i++) {
+		*time_sel = gts->itime_table[i].sel;
+		ret = iio_gts_find_gain_sel_for_scale_using_time(gts, *time_sel,
+								 scale_int,
+								 scale_nano,
+								 gain_sel);
+		if (ret)
+			continue;
+
+		return 0;
+	}
+
+	return -EINVAL;
+}
+EXPORT_SYMBOL_NS_GPL(iio_gts_find_gain_time_sel_for_scale, "IIO_GTS_HELPER");
 
-static int iio_gts_get_total_gain(struct iio_gts *gts, int gain, int time)
+/**
+ * iio_gts_get_total_gain - Fetch total gain for given HW-gain and time
+ * @gts:	Gain time scale descriptor
+ * @gain:	HW-gain for which the total gain is searched for
+ * @time:	Integration time for which the total gain is searched for
+ *
+ * Return: total gain on success and -EINVAL on error.
+ */
+int iio_gts_get_total_gain(struct iio_gts *gts, int gain, int time)
 {
 	const struct iio_itime_sel_mul *itime;
 
@@ -939,6 +1050,7 @@ static int iio_gts_get_total_gain(struct iio_gts *gts, int gain, int time)
 
 	return gain * itime->mul;
 }
+EXPORT_SYMBOL_NS_GPL(iio_gts_get_total_gain, "IIO_GTS_HELPER");
 
 static int iio_gts_get_scale_linear(struct iio_gts *gts, int gain, int time,
 				    u64 *scale)
@@ -983,7 +1095,7 @@ int iio_gts_get_scale(struct iio_gts *gts, int gain, int time, int *scale_int,
 
 	return iio_gts_delinearize(lin_scale, NANO, scale_int, scale_nano);
 }
-EXPORT_SYMBOL_NS_GPL(iio_gts_get_scale, IIO_GTS_HELPER);
+EXPORT_SYMBOL_NS_GPL(iio_gts_get_scale, "IIO_GTS_HELPER");
 
 /**
  * iio_gts_find_new_gain_sel_by_old_gain_time - compensate for time change
@@ -1040,7 +1152,7 @@ int iio_gts_find_new_gain_sel_by_old_gain_time(struct iio_gts *gts,
 
 	return 0;
 }
-EXPORT_SYMBOL_NS_GPL(iio_gts_find_new_gain_sel_by_old_gain_time, IIO_GTS_HELPER);
+EXPORT_SYMBOL_NS_GPL(iio_gts_find_new_gain_sel_by_old_gain_time, "IIO_GTS_HELPER");
 
 /**
  * iio_gts_find_new_gain_by_old_gain_time - compensate for time change
@@ -1092,7 +1204,49 @@ int iio_gts_find_new_gain_by_old_gain_time(struct iio_gts *gts, int old_gain,
 
 	return 0;
 }
-EXPORT_SYMBOL_NS_GPL(iio_gts_find_new_gain_by_old_gain_time, IIO_GTS_HELPER);
+EXPORT_SYMBOL_NS_GPL(iio_gts_find_new_gain_by_old_gain_time, "IIO_GTS_HELPER");
+
+/**
+ * iio_gts_find_new_gain_by_gain_time_min - compensate for time change
+ * @gts:	Gain time scale descriptor
+ * @old_gain:	Previously set gain
+ * @old_time:	Selector corresponding previously set time
+ * @new_time:	Selector corresponding new time to be set
+ * @new_gain:	Pointer to value where new gain is to be written
+ * @in_range:	Indicate if the @new_gain was in the range of
+ *		supported gains.
+ *
+ * Wrapper around iio_gts_find_new_gain_by_old_gain_time() that tries to
+ * set an optimal value if no exact match was found, defaulting to the
+ * minimum gain to avoid saturations if the optimal value is not in the
+ * range of supported gains.
+ *
+ * Return: 0 on success and a negative value if no gain was found.
+ */
+int iio_gts_find_new_gain_by_gain_time_min(struct iio_gts *gts, int old_gain,
+					   int old_time, int new_time,
+					   int *new_gain, bool *in_range)
+{
+	int ret;
+
+	*in_range = true;
+	ret = iio_gts_find_new_gain_by_old_gain_time(gts, old_gain, old_time,
+						     new_time, new_gain);
+	if (*new_gain < 0)
+		return -EINVAL;
+
+	if (ret) {
+		*new_gain = iio_find_closest_gain_low(gts, *new_gain, in_range);
+		if (*new_gain < 0) {
+			*new_gain = iio_gts_get_min_gain(gts);
+			if (*new_gain < 0)
+				return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL_NS_GPL(iio_gts_find_new_gain_by_gain_time_min, "IIO_GTS_HELPER");
 
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Matti Vaittinen <mazziesaccount@gmail.com>");