power: fg-util: add median filter for circular buffer

A median filter is useful for filtering out outliers. Add it.

Change-Id: I21f97a870c262e5fb3d33b8250a2bf074f519b58
Signed-off-by: Nicholas Troast <ntroast@codeaurora.org>
Signed-off-by: Abhijeet Dharmapurikar <adharmap@codeaurora.org>

drivers/power/supply/qcom/fg-core.h

@@ -311,7 +311,7 @@ struct fg_irq_info {
 };
 
 struct fg_circ_buf {
-	int	arr[20];
+	int	arr[10];
 	int	size;
 	int	head;
 };
@@ -483,5 +483,6 @@ extern bool is_qnovo_en(struct fg_chip *chip);
 extern void fg_circ_buf_add(struct fg_circ_buf *, int);
 extern void fg_circ_buf_clr(struct fg_circ_buf *);
 extern int fg_circ_buf_avg(struct fg_circ_buf *, int *);
+extern int fg_circ_buf_median(struct fg_circ_buf *, int *);
 extern int fg_lerp(const struct fg_pt *, size_t, s32, s32 *);
 #endif

drivers/power/supply/qcom/fg-util.c

@@ -10,6 +10,7 @@
  * GNU General Public License for more details.
  */
 
+#include <linux/sort.h>
 #include "fg-core.h"
 
 void fg_circ_buf_add(struct fg_circ_buf *buf, int val)
@@ -39,6 +40,39 @@ int fg_circ_buf_avg(struct fg_circ_buf *buf, int *avg)
 	return 0;
 }
 
+static int cmp_int(const void *a, const void *b)
+{
+	return *(int *)a - *(int *)b;
+}
+
+int fg_circ_buf_median(struct fg_circ_buf *buf, int *median)
+{
+	int *temp;
+
+	if (buf->size == 0)
+		return -ENODATA;
+
+	if (buf->size == 1) {
+		*median = buf->arr[0];
+		return 0;
+	}
+
+	temp = kmalloc_array(buf->size, sizeof(*temp), GFP_KERNEL);
+	if (!temp)
+		return -ENOMEM;
+
+	memcpy(temp, buf->arr, buf->size * sizeof(*temp));
+	sort(temp, buf->size, sizeof(*temp), cmp_int, NULL);
+
+	if (buf->size % 2)
+		*median = temp[buf->size / 2];
+	else
+		*median = (temp[buf->size / 2 - 1] + temp[buf->size / 2]) / 2;
+
+	kfree(temp);
+	return 0;
+}
+
 int fg_lerp(const struct fg_pt *pts, size_t tablesize, s32 input, s32 *output)
 {
 	int i;