1
2
3
4
5
6
7
8
9#define pr_fmt(fmt) "Power allocator: " fmt
10
11#include <linux/rculist.h>
12#include <linux/slab.h>
13#include <linux/thermal.h>
14
15#define CREATE_TRACE_POINTS
16#include <trace/events/thermal_power_allocator.h>
17
18#include "thermal_core.h"
19
20#define INVALID_TRIP -1
21
22#define FRAC_BITS 10
23#define int_to_frac(x) ((x) << FRAC_BITS)
24#define frac_to_int(x) ((x) >> FRAC_BITS)
25
26
27
28
29
30
31
32
33
34static inline s64 mul_frac(s64 x, s64 y)
35{
36 return (x * y) >> FRAC_BITS;
37}
38
39
40
41
42
43
44
45
46
47static inline s64 div_frac(s64 x, s64 y)
48{
49 return div_s64(x << FRAC_BITS, y);
50}
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69struct power_allocator_params {
70 bool allocated_tzp;
71 s64 err_integral;
72 s32 prev_err;
73 int trip_switch_on;
74 int trip_max_desired_temperature;
75 u32 sustainable_power;
76};
77
78
79
80
81
82
83
84
85
86
87
88
89static u32 estimate_sustainable_power(struct thermal_zone_device *tz)
90{
91 u32 sustainable_power = 0;
92 struct thermal_instance *instance;
93 struct power_allocator_params *params = tz->governor_data;
94
95 list_for_each_entry(instance, &tz->thermal_instances, tz_node) {
96 struct thermal_cooling_device *cdev = instance->cdev;
97 u32 min_power;
98
99 if (instance->trip != params->trip_max_desired_temperature)
100 continue;
101
102 if (!cdev_is_power_actor(cdev))
103 continue;
104
105 if (cdev->ops->state2power(cdev, instance->upper, &min_power))
106 continue;
107
108 sustainable_power += min_power;
109 }
110
111 return sustainable_power;
112}
113
114
115
116
117
118
119
120
121
122
123
124static void estimate_pid_constants(struct thermal_zone_device *tz,
125 u32 sustainable_power, int trip_switch_on,
126 int control_temp)
127{
128 int ret;
129 int switch_on_temp;
130 u32 temperature_threshold;
131 s32 k_i;
132
133 ret = tz->ops->get_trip_temp(tz, trip_switch_on, &switch_on_temp);
134 if (ret)
135 switch_on_temp = 0;
136
137 temperature_threshold = control_temp - switch_on_temp;
138
139
140
141
142
143
144
145
146 if (!temperature_threshold)
147 return;
148
149 tz->tzp->k_po = int_to_frac(sustainable_power) /
150 temperature_threshold;
151
152 tz->tzp->k_pu = int_to_frac(2 * sustainable_power) /
153 temperature_threshold;
154
155 k_i = tz->tzp->k_pu / 10;
156 tz->tzp->k_i = k_i > 0 ? k_i : 1;
157
158
159
160
161
162}
163
164
165
166
167
168
169
170
171
172
173
174
175static u32 get_sustainable_power(struct thermal_zone_device *tz,
176 struct power_allocator_params *params,
177 int control_temp)
178{
179 u32 sustainable_power;
180
181 if (!tz->tzp->sustainable_power)
182 sustainable_power = estimate_sustainable_power(tz);
183 else
184 sustainable_power = tz->tzp->sustainable_power;
185
186
187 if (sustainable_power != params->sustainable_power) {
188 estimate_pid_constants(tz, sustainable_power,
189 params->trip_switch_on, control_temp);
190
191
192 tz->tzp->sustainable_power = sustainable_power;
193 params->sustainable_power = sustainable_power;
194 }
195
196 return sustainable_power;
197}
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217static u32 pid_controller(struct thermal_zone_device *tz,
218 int control_temp,
219 u32 max_allocatable_power)
220{
221 s64 p, i, d, power_range;
222 s32 err, max_power_frac;
223 u32 sustainable_power;
224 struct power_allocator_params *params = tz->governor_data;
225
226 max_power_frac = int_to_frac(max_allocatable_power);
227
228 sustainable_power = get_sustainable_power(tz, params, control_temp);
229
230 err = control_temp - tz->temperature;
231 err = int_to_frac(err);
232
233
234 p = mul_frac(err < 0 ? tz->tzp->k_po : tz->tzp->k_pu, err);
235
236
237
238
239
240
241
242 i = mul_frac(tz->tzp->k_i, params->err_integral);
243
244 if (err < int_to_frac(tz->tzp->integral_cutoff)) {
245 s64 i_next = i + mul_frac(tz->tzp->k_i, err);
246
247 if (abs(i_next) < max_power_frac) {
248 i = i_next;
249 params->err_integral += err;
250 }
251 }
252
253
254
255
256
257
258
259
260 d = mul_frac(tz->tzp->k_d, err - params->prev_err);
261 d = div_frac(d, jiffies_to_msecs(tz->passive_delay_jiffies));
262 params->prev_err = err;
263
264 power_range = p + i + d;
265
266
267 power_range = sustainable_power + frac_to_int(power_range);
268
269 power_range = clamp(power_range, (s64)0, (s64)max_allocatable_power);
270
271 trace_thermal_power_allocator_pid(tz, frac_to_int(err),
272 frac_to_int(params->err_integral),
273 frac_to_int(p), frac_to_int(i),
274 frac_to_int(d), power_range);
275
276 return power_range;
277}
278
279
280
281
282
283
284
285
286
287
288
289
290
291static int
292power_actor_set_power(struct thermal_cooling_device *cdev,
293 struct thermal_instance *instance, u32 power)
294{
295 unsigned long state;
296 int ret;
297
298 ret = cdev->ops->power2state(cdev, power, &state);
299 if (ret)
300 return ret;
301
302 instance->target = clamp_val(state, instance->lower, instance->upper);
303 mutex_lock(&cdev->lock);
304 __thermal_cdev_update(cdev);
305 mutex_unlock(&cdev->lock);
306
307 return 0;
308}
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337static void divvy_up_power(u32 *req_power, u32 *max_power, int num_actors,
338 u32 total_req_power, u32 power_range,
339 u32 *granted_power, u32 *extra_actor_power)
340{
341 u32 extra_power, capped_extra_power;
342 int i;
343
344
345
346
347 if (!total_req_power)
348 total_req_power = 1;
349
350 capped_extra_power = 0;
351 extra_power = 0;
352 for (i = 0; i < num_actors; i++) {
353 u64 req_range = (u64)req_power[i] * power_range;
354
355 granted_power[i] = DIV_ROUND_CLOSEST_ULL(req_range,
356 total_req_power);
357
358 if (granted_power[i] > max_power[i]) {
359 extra_power += granted_power[i] - max_power[i];
360 granted_power[i] = max_power[i];
361 }
362
363 extra_actor_power[i] = max_power[i] - granted_power[i];
364 capped_extra_power += extra_actor_power[i];
365 }
366
367 if (!extra_power)
368 return;
369
370
371
372
373
374 extra_power = min(extra_power, capped_extra_power);
375 if (capped_extra_power > 0)
376 for (i = 0; i < num_actors; i++) {
377 u64 extra_range = (u64)extra_actor_power[i] * extra_power;
378 granted_power[i] += DIV_ROUND_CLOSEST_ULL(extra_range,
379 capped_extra_power);
380 }
381}
382
383static int allocate_power(struct thermal_zone_device *tz,
384 int control_temp)
385{
386 struct thermal_instance *instance;
387 struct power_allocator_params *params = tz->governor_data;
388 u32 *req_power, *max_power, *granted_power, *extra_actor_power;
389 u32 *weighted_req_power;
390 u32 total_req_power, max_allocatable_power, total_weighted_req_power;
391 u32 total_granted_power, power_range;
392 int i, num_actors, total_weight, ret = 0;
393 int trip_max_desired_temperature = params->trip_max_desired_temperature;
394
395 mutex_lock(&tz->lock);
396
397 num_actors = 0;
398 total_weight = 0;
399 list_for_each_entry(instance, &tz->thermal_instances, tz_node) {
400 if ((instance->trip == trip_max_desired_temperature) &&
401 cdev_is_power_actor(instance->cdev)) {
402 num_actors++;
403 total_weight += instance->weight;
404 }
405 }
406
407 if (!num_actors) {
408 ret = -ENODEV;
409 goto unlock;
410 }
411
412
413
414
415
416
417
418
419 BUILD_BUG_ON(sizeof(*req_power) != sizeof(*max_power));
420 BUILD_BUG_ON(sizeof(*req_power) != sizeof(*granted_power));
421 BUILD_BUG_ON(sizeof(*req_power) != sizeof(*extra_actor_power));
422 BUILD_BUG_ON(sizeof(*req_power) != sizeof(*weighted_req_power));
423 req_power = kcalloc(num_actors * 5, sizeof(*req_power), GFP_KERNEL);
424 if (!req_power) {
425 ret = -ENOMEM;
426 goto unlock;
427 }
428
429 max_power = &req_power[num_actors];
430 granted_power = &req_power[2 * num_actors];
431 extra_actor_power = &req_power[3 * num_actors];
432 weighted_req_power = &req_power[4 * num_actors];
433
434 i = 0;
435 total_weighted_req_power = 0;
436 total_req_power = 0;
437 max_allocatable_power = 0;
438
439 list_for_each_entry(instance, &tz->thermal_instances, tz_node) {
440 int weight;
441 struct thermal_cooling_device *cdev = instance->cdev;
442
443 if (instance->trip != trip_max_desired_temperature)
444 continue;
445
446 if (!cdev_is_power_actor(cdev))
447 continue;
448
449 if (cdev->ops->get_requested_power(cdev, &req_power[i]))
450 continue;
451
452 if (!total_weight)
453 weight = 1 << FRAC_BITS;
454 else
455 weight = instance->weight;
456
457 weighted_req_power[i] = frac_to_int(weight * req_power[i]);
458
459 if (cdev->ops->state2power(cdev, instance->lower,
460 &max_power[i]))
461 continue;
462
463 total_req_power += req_power[i];
464 max_allocatable_power += max_power[i];
465 total_weighted_req_power += weighted_req_power[i];
466
467 i++;
468 }
469
470 power_range = pid_controller(tz, control_temp, max_allocatable_power);
471
472 divvy_up_power(weighted_req_power, max_power, num_actors,
473 total_weighted_req_power, power_range, granted_power,
474 extra_actor_power);
475
476 total_granted_power = 0;
477 i = 0;
478 list_for_each_entry(instance, &tz->thermal_instances, tz_node) {
479 if (instance->trip != trip_max_desired_temperature)
480 continue;
481
482 if (!cdev_is_power_actor(instance->cdev))
483 continue;
484
485 power_actor_set_power(instance->cdev, instance,
486 granted_power[i]);
487 total_granted_power += granted_power[i];
488
489 i++;
490 }
491
492 trace_thermal_power_allocator(tz, req_power, total_req_power,
493 granted_power, total_granted_power,
494 num_actors, power_range,
495 max_allocatable_power, tz->temperature,
496 control_temp - tz->temperature);
497
498 kfree(req_power);
499unlock:
500 mutex_unlock(&tz->lock);
501
502 return ret;
503}
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520static void get_governor_trips(struct thermal_zone_device *tz,
521 struct power_allocator_params *params)
522{
523 int i, last_active, last_passive;
524 bool found_first_passive;
525
526 found_first_passive = false;
527 last_active = INVALID_TRIP;
528 last_passive = INVALID_TRIP;
529
530 for (i = 0; i < tz->trips; i++) {
531 enum thermal_trip_type type;
532 int ret;
533
534 ret = tz->ops->get_trip_type(tz, i, &type);
535 if (ret) {
536 dev_warn(&tz->device,
537 "Failed to get trip point %d type: %d\n", i,
538 ret);
539 continue;
540 }
541
542 if (type == THERMAL_TRIP_PASSIVE) {
543 if (!found_first_passive) {
544 params->trip_switch_on = i;
545 found_first_passive = true;
546 } else {
547 last_passive = i;
548 }
549 } else if (type == THERMAL_TRIP_ACTIVE) {
550 last_active = i;
551 } else {
552 break;
553 }
554 }
555
556 if (last_passive != INVALID_TRIP) {
557 params->trip_max_desired_temperature = last_passive;
558 } else if (found_first_passive) {
559 params->trip_max_desired_temperature = params->trip_switch_on;
560 params->trip_switch_on = INVALID_TRIP;
561 } else {
562 params->trip_switch_on = INVALID_TRIP;
563 params->trip_max_desired_temperature = last_active;
564 }
565}
566
567static void reset_pid_controller(struct power_allocator_params *params)
568{
569 params->err_integral = 0;
570 params->prev_err = 0;
571}
572
573static void allow_maximum_power(struct thermal_zone_device *tz, bool update)
574{
575 struct thermal_instance *instance;
576 struct power_allocator_params *params = tz->governor_data;
577 u32 req_power;
578
579 mutex_lock(&tz->lock);
580 list_for_each_entry(instance, &tz->thermal_instances, tz_node) {
581 struct thermal_cooling_device *cdev = instance->cdev;
582
583 if ((instance->trip != params->trip_max_desired_temperature) ||
584 (!cdev_is_power_actor(instance->cdev)))
585 continue;
586
587 instance->target = 0;
588 mutex_lock(&instance->cdev->lock);
589
590
591
592
593
594 cdev->ops->get_requested_power(cdev, &req_power);
595
596 if (update)
597 __thermal_cdev_update(instance->cdev);
598
599 mutex_unlock(&instance->cdev->lock);
600 }
601 mutex_unlock(&tz->lock);
602}
603
604
605
606
607
608
609
610
611
612
613
614
615
616static int check_power_actors(struct thermal_zone_device *tz)
617{
618 struct thermal_instance *instance;
619 int ret = 0;
620
621 list_for_each_entry(instance, &tz->thermal_instances, tz_node) {
622 if (!cdev_is_power_actor(instance->cdev)) {
623 dev_warn(&tz->device, "power_allocator: %s is not a power actor\n",
624 instance->cdev->type);
625 ret = -EINVAL;
626 }
627 }
628
629 return ret;
630}
631
632
633
634
635
636
637
638
639
640
641
642static int power_allocator_bind(struct thermal_zone_device *tz)
643{
644 int ret;
645 struct power_allocator_params *params;
646 int control_temp;
647
648 ret = check_power_actors(tz);
649 if (ret)
650 return ret;
651
652 params = kzalloc(sizeof(*params), GFP_KERNEL);
653 if (!params)
654 return -ENOMEM;
655
656 if (!tz->tzp) {
657 tz->tzp = kzalloc(sizeof(*tz->tzp), GFP_KERNEL);
658 if (!tz->tzp) {
659 ret = -ENOMEM;
660 goto free_params;
661 }
662
663 params->allocated_tzp = true;
664 }
665
666 if (!tz->tzp->sustainable_power)
667 dev_warn(&tz->device, "power_allocator: sustainable_power will be estimated\n");
668
669 get_governor_trips(tz, params);
670
671 if (tz->trips > 0) {
672 ret = tz->ops->get_trip_temp(tz,
673 params->trip_max_desired_temperature,
674 &control_temp);
675 if (!ret)
676 estimate_pid_constants(tz, tz->tzp->sustainable_power,
677 params->trip_switch_on,
678 control_temp);
679 }
680
681 reset_pid_controller(params);
682
683 tz->governor_data = params;
684
685 return 0;
686
687free_params:
688 kfree(params);
689
690 return ret;
691}
692
693static void power_allocator_unbind(struct thermal_zone_device *tz)
694{
695 struct power_allocator_params *params = tz->governor_data;
696
697 dev_dbg(&tz->device, "Unbinding from thermal zone %d\n", tz->id);
698
699 if (params->allocated_tzp) {
700 kfree(tz->tzp);
701 tz->tzp = NULL;
702 }
703
704 kfree(tz->governor_data);
705 tz->governor_data = NULL;
706}
707
708static int power_allocator_throttle(struct thermal_zone_device *tz, int trip)
709{
710 int ret;
711 int switch_on_temp, control_temp;
712 struct power_allocator_params *params = tz->governor_data;
713 bool update;
714
715
716
717
718
719 if (trip != params->trip_max_desired_temperature)
720 return 0;
721
722 ret = tz->ops->get_trip_temp(tz, params->trip_switch_on,
723 &switch_on_temp);
724 if (!ret && (tz->temperature < switch_on_temp)) {
725 update = (tz->last_temperature >= switch_on_temp);
726 tz->passive = 0;
727 reset_pid_controller(params);
728 allow_maximum_power(tz, update);
729 return 0;
730 }
731
732 tz->passive = 1;
733
734 ret = tz->ops->get_trip_temp(tz, params->trip_max_desired_temperature,
735 &control_temp);
736 if (ret) {
737 dev_warn(&tz->device,
738 "Failed to get the maximum desired temperature: %d\n",
739 ret);
740 return ret;
741 }
742
743 return allocate_power(tz, control_temp);
744}
745
746static struct thermal_governor thermal_gov_power_allocator = {
747 .name = "power_allocator",
748 .bind_to_tz = power_allocator_bind,
749 .unbind_from_tz = power_allocator_unbind,
750 .throttle = power_allocator_throttle,
751};
752THERMAL_GOVERNOR_DECLARE(thermal_gov_power_allocator);
753