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 
wf_pid_init(struct wf_pid_state * st,struct wf_pid_param * param)25 void 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 }
31 EXPORT_SYMBOL_GPL(wf_pid_init);
32 
wf_pid_run(struct wf_pid_state * st,s32 new_sample)33 s32 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 }
77 EXPORT_SYMBOL_GPL(wf_pid_run);
78 
wf_cpu_pid_init(struct wf_cpu_pid_state * st,struct wf_cpu_pid_param * param)79 void 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 }
86 EXPORT_SYMBOL_GPL(wf_cpu_pid_init);
87 
wf_cpu_pid_run(struct wf_cpu_pid_state * st,s32 new_power,s32 new_temp)88 s32 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 }
144 EXPORT_SYMBOL_GPL(wf_cpu_pid_run);
145 
146 MODULE_AUTHOR("Benjamin Herrenschmidt <benh@kernel.crashing.org>");
147 MODULE_DESCRIPTION("PID algorithm for PowerMacs thermal control");
148 MODULE_LICENSE("GPL");
149