1// SPDX-License-Identifier: GPL-2.0-only
2/*
3 * Windfarm PowerMac thermal control. Generic PID helpers
4 *
5 * (c) Copyright 2005 Benjamin Herrenschmidt, IBM Corp.
6 *                    <benh@kernel.crashing.org>
7 */
8
9#include <linux/types.h>
10#include <linux/errno.h>
11#include <linux/kernel.h>
12#include <linux/string.h>
13#include <linux/module.h>
14
15#include "windfarm_pid.h"
16
17#undef DEBUG
18
19#ifdef DEBUG
20#define DBG(args...)	printk(args)
21#else
22#define DBG(args...)	do { } while(0)
23#endif
24
25void wf_pid_init(struct wf_pid_state *st, struct wf_pid_param *param)
26{
27	memset(st, 0, sizeof(struct wf_pid_state));
28	st->param = *param;
29	st->first = 1;
30}
31EXPORT_SYMBOL_GPL(wf_pid_init);
32
33s32 wf_pid_run(struct wf_pid_state *st, s32 new_sample)
34{
35	s64	error, integ, deriv;
36	s32	target;
37	int	i, hlen = st->param.history_len;
38
39	/* Calculate error term */
40	error = new_sample - st->param.itarget;
41
42	/* Get samples into our history buffer */
43	if (st->first) {
44		for (i = 0; i < hlen; i++) {
45			st->samples[i] = new_sample;
46			st->errors[i] = error;
47		}
48		st->first = 0;
49		st->index = 0;
50	} else {
51		st->index = (st->index + 1) % hlen;
52		st->samples[st->index] = new_sample;
53		st->errors[st->index] = error;
54	}
55
56	/* Calculate integral term */
57	for (i = 0, integ = 0; i < hlen; i++)
58		integ += st->errors[(st->index + hlen - i) % hlen];
59	integ *= st->param.interval;
60
61	/* Calculate derivative term */
62	deriv = st->errors[st->index] -
63		st->errors[(st->index + hlen - 1) % hlen];
64	deriv /= st->param.interval;
65
66	/* Calculate target */
67	target = (s32)((integ * (s64)st->param.gr + deriv * (s64)st->param.gd +
68		  error * (s64)st->param.gp) >> 36);
69	if (st->param.additive)
70		target += st->target;
71	target = max(target, st->param.min);
72	target = min(target, st->param.max);
73	st->target = target;
74
75	return st->target;
76}
77EXPORT_SYMBOL_GPL(wf_pid_run);
78
79void wf_cpu_pid_init(struct wf_cpu_pid_state *st,
80		     struct wf_cpu_pid_param *param)
81{
82	memset(st, 0, sizeof(struct wf_cpu_pid_state));
83	st->param = *param;
84	st->first = 1;
85}
86EXPORT_SYMBOL_GPL(wf_cpu_pid_init);
87
88s32 wf_cpu_pid_run(struct wf_cpu_pid_state *st, s32 new_power, s32 new_temp)
89{
90	s64	integ, deriv, prop;
91	s32	error, target, sval, adj;
92	int	i, hlen = st->param.history_len;
93
94	/* Calculate error term */
95	error = st->param.pmaxadj - new_power;
96
97	/* Get samples into our history buffer */
98	if (st->first) {
99		for (i = 0; i < hlen; i++) {
100			st->powers[i] = new_power;
101			st->errors[i] = error;
102		}
103		st->temps[0] = st->temps[1] = new_temp;
104		st->first = 0;
105		st->index = st->tindex = 0;
106	} else {
107		st->index = (st->index + 1) % hlen;
108		st->powers[st->index] = new_power;
109		st->errors[st->index] = error;
110		st->tindex = (st->tindex + 1) % 2;
111		st->temps[st->tindex] = new_temp;
112	}
113
114	/* Calculate integral term */
115	for (i = 0, integ = 0; i < hlen; i++)
116		integ += st->errors[(st->index + hlen - i) % hlen];
117	integ *= st->param.interval;
118	integ *= st->param.gr;
119	sval = st->param.tmax - (s32)(integ >> 20);
120	adj = min(st->param.ttarget, sval);
121
122	DBG("integ: %lx, sval: %lx, adj: %lx\n", integ, sval, adj);
123
124	/* Calculate derivative term */
125	deriv = st->temps[st->tindex] -
126		st->temps[(st->tindex + 2 - 1) % 2];
127	deriv /= st->param.interval;
128	deriv *= st->param.gd;
129
130	/* Calculate proportional term */
131	prop = st->last_delta = (new_temp - adj);
132	prop *= st->param.gp;
133
134	DBG("deriv: %lx, prop: %lx\n", deriv, prop);
135
136	/* Calculate target */
137	target = st->target + (s32)((deriv + prop) >> 36);
138	target = max(target, st->param.min);
139	target = min(target, st->param.max);
140	st->target = target;
141
142	return st->target;
143}
144EXPORT_SYMBOL_GPL(wf_cpu_pid_run);
145
146MODULE_AUTHOR("Benjamin Herrenschmidt <benh@kernel.crashing.org>");
147MODULE_DESCRIPTION("PID algorithm for PowerMacs thermal control");
148MODULE_LICENSE("GPL");
149