winterhold: Implement custom fan duty adjustment logic

It's strongly based on the existing logic from zephyr/shim/src/fan.c.
The main difference is that the fan duty is changed less aggressively,
in order to avoid fan oscillation. This is done by implementing
a "grace period" functionality. After the duty is change the controller
will wait for a few ticks, depending on how big the change was.
This is a quick fix, at some point it should be replaced by a PID type
of algorithm.

BUG=b:276577324
BRANCH=none
TEST=Verify that the fan doesn't ramp up and down all the time under
     various scenarios, e.g. watching multiple videos on youtube.

Co-developed-by: Radosław Biernacki <biernacki@google.com>
Change-Id: I893a7802df8f5271fe1a253272fdcc3149bdf178
Signed-off-by: Kornel Dulęba <korneld@google.com>
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/4423153
Reviewed-by: Keith Short <keithshort@chromium.org>
Reviewed-by: Radoslaw Biernacki <biernacki@google.com>
Reviewed-by: Diana Z <dzigterman@chromium.org>
(cherry picked from commit 17b095b064576c66e8556e6bb194ab8e8d23f412)
Reviewed-on: https://chromium-review.googlesource.com/c/chromiumos/platform/ec/+/4459716
Commit-Queue: Chao Gui <chaogui@google.com>

3 files changed, 85 insertions, 0 deletions

diff --git a/zephyr/program/skyrim/winterhold/project.conf b/zephyr/program/skyrim/winterhold/project.conf
index 27f1c60c88..3521417c68 100644
--- a/zephyr/program/skyrim/winterhold/project.conf
+++ b/zephyr/program/skyrim/winterhold/project.conf
@@ -51,3 +51,6 @@ CONFIG_PLATFORM_EC_GESTURE_HOST_DETECTION=y
 
 # GPIO configuration
 CONFIG_GPIO_GET_CONFIG=y
+
+# FAN
+CONFIG_PLATFORM_EC_CUSTOM_FAN_DUTY_CONTROL=y
diff --git a/zephyr/program/skyrim/winterhold/project.overlay b/zephyr/program/skyrim/winterhold/project.overlay
index a348b0f55c..818c625fe9 100644
--- a/zephyr/program/skyrim/winterhold/project.overlay
+++ b/zephyr/program/skyrim/winterhold/project.overlay
@@ -197,6 +197,7 @@
 	rpm_min = <2100>;
 	rpm_start = <2800>;
 	rpm_max = <4800>;
+	rpm_deviation = <3>;
 };
 
 /* temperature sensor overrides */
diff --git a/zephyr/program/skyrim/winterhold/src/thermal.c b/zephyr/program/skyrim/winterhold/src/thermal.c
index 0ccd9dd9e2..00e162e45a 100644
--- a/zephyr/program/skyrim/winterhold/src/thermal.c
+++ b/zephyr/program/skyrim/winterhold/src/thermal.c
@@ -4,9 +4,11 @@
  */
 
 #include "body_detection.h"
+#include "fan.h"
 #include "hooks.h"
 #include "host_command.h"
 #include "lid_switch.h"
+#include "math_util.h"
 #include "temp_sensor/temp_sensor.h"
 #include "thermal.h"
 
@@ -209,3 +211,82 @@ static void detect_temp_change(void)
 	}
 }
 DECLARE_HOOK(HOOK_SECOND, detect_temp_change, HOOK_PRIO_TEMP_SENSOR_DONE);
+
+#ifdef CONFIG_PLATFORM_EC_CUSTOM_FAN_DUTY_CONTROL
+
+K_TIMER_DEFINE(grace_period_timer, NULL, NULL);
+
+enum fan_status board_override_fan_control_duty(int ch)
+{
+	int duty, rpm_diff, deviation, duty_step;
+	struct fan_data *data = &fan_data[ch];
+	int rpm_actual = data->rpm_actual;
+	int rpm_target = data->rpm_target;
+
+	/* This works with one fan only. */
+	if (ch != 0) {
+		CPRINTS("Only FAN0 is supported!");
+		return FAN_STATUS_FRUSTRATED;
+	}
+
+	/* Wait for fan RPM to catch up after its duty has been changed. */
+	if (k_timer_remaining_ticks(&grace_period_timer) != 0)
+		return FAN_STATUS_LOCKED;
+
+	duty = fan_get_duty(ch);
+	if (duty == 0 && rpm_target == 0)
+		return FAN_STATUS_STOPPED;
+
+	/*
+	 * If the current RPM is close enough to the target just leave it.
+	 * It's always going to fluctuate a bit anyway.
+	 */
+	deviation = fans[ch].rpm->rpm_deviation * rpm_target / 100;
+	rpm_diff = rpm_target - rpm_actual;
+	if (rpm_diff > deviation) {
+		/* Can't set duty higher than 100%... */
+		if (duty == 100)
+			return FAN_STATUS_FRUSTRATED;
+	} else if (rpm_diff < -deviation) {
+		/* Can't set duty lower than 1%... */
+		if (duty == 1 && rpm_target != 0)
+			return FAN_STATUS_FRUSTRATED;
+	} else {
+		return FAN_STATUS_LOCKED;
+	}
+
+	/*
+	 * The rpm_diff -> duty_step conversion is specific to a specific
+	 * whiterun fan.
+	 * It has been determined empirically.
+	 */
+	if (ABS(rpm_diff) >= 2500) {
+		duty_step = 35;
+		k_timer_start(&grace_period_timer, K_MSEC(800), K_NO_WAIT);
+	} else if (ABS(rpm_diff) >= 2000) {
+		duty_step = 28;
+		k_timer_start(&grace_period_timer, K_MSEC(800), K_NO_WAIT);
+	} else if (ABS(rpm_diff) >= 1000) {
+		duty_step = 14;
+		k_timer_start(&grace_period_timer, K_MSEC(800), K_NO_WAIT);
+	} else if (ABS(rpm_diff) >= 500) {
+		duty_step = 6;
+		k_timer_start(&grace_period_timer, K_MSEC(800), K_NO_WAIT);
+	} else if (ABS(rpm_diff) >= 250) {
+		duty_step = 3;
+		k_timer_start(&grace_period_timer, K_MSEC(600), K_NO_WAIT);
+	} else {
+		duty_step = 1;
+		k_timer_start(&grace_period_timer, K_MSEC(600), K_NO_WAIT);
+	}
+
+	if (rpm_diff > 0)
+		duty = MIN(duty + duty_step, 100);
+	else
+		duty = MAX(duty - duty_step, 1);
+
+	fan_set_duty(ch, duty);
+
+	return FAN_STATUS_CHANGING;
+}
+#endif