1 // SPDX-License-Identifier: GPL-2.0
2 /*
3  * SCMI Powercap support.
4  *
5  * Copyright (C) 2022 ARM Ltd.
6  */
7 
8 #include <linux/device.h>
9 #include <linux/math.h>
10 #include <linux/limits.h>
11 #include <linux/list.h>
12 #include <linux/module.h>
13 #include <linux/powercap.h>
14 #include <linux/scmi_protocol.h>
15 #include <linux/slab.h>
16 
17 #define to_scmi_powercap_zone(z)		\
18 	container_of(z, struct scmi_powercap_zone, zone)
19 
20 static const struct scmi_powercap_proto_ops *powercap_ops;
21 
22 struct scmi_powercap_zone {
23 	bool registered;
24 	bool invalid;
25 	unsigned int height;
26 	struct device *dev;
27 	struct scmi_protocol_handle *ph;
28 	const struct scmi_powercap_info *info;
29 	struct scmi_powercap_zone *spzones;
30 	struct powercap_zone zone;
31 	struct list_head node;
32 };
33 
34 struct scmi_powercap_root {
35 	unsigned int num_zones;
36 	struct scmi_powercap_zone *spzones;
37 	struct list_head *registered_zones;
38 	struct list_head scmi_zones;
39 };
40 
41 static struct powercap_control_type *scmi_top_pcntrl;
42 
43 static int scmi_powercap_zone_release(struct powercap_zone *pz)
44 {
45 	return 0;
46 }
47 
48 static int scmi_powercap_get_max_power_range_uw(struct powercap_zone *pz,
49 						u64 *max_power_range_uw)
50 {
51 	*max_power_range_uw = U32_MAX;
52 	return 0;
53 }
54 
55 static int scmi_powercap_get_power_uw(struct powercap_zone *pz,
56 				      u64 *power_uw)
57 {
58 	struct scmi_powercap_zone *spz = to_scmi_powercap_zone(pz);
59 	u32 avg_power, pai;
60 	int ret;
61 
62 	if (!spz->info->powercap_monitoring)
63 		return -EINVAL;
64 
65 	ret = powercap_ops->measurements_get(spz->ph, spz->info->id, &avg_power,
66 					     &pai);
67 	if (ret)
68 		return ret;
69 
70 	*power_uw = avg_power;
71 	if (spz->info->powercap_scale_mw)
72 		*power_uw *= 1000;
73 
74 	return 0;
75 }
76 
77 static int scmi_powercap_zone_enable_set(struct powercap_zone *pz, bool mode)
78 {
79 	struct scmi_powercap_zone *spz = to_scmi_powercap_zone(pz);
80 
81 	return powercap_ops->cap_enable_set(spz->ph, spz->info->id, mode);
82 }
83 
84 static int scmi_powercap_zone_enable_get(struct powercap_zone *pz, bool *mode)
85 {
86 	struct scmi_powercap_zone *spz = to_scmi_powercap_zone(pz);
87 
88 	return powercap_ops->cap_enable_get(spz->ph, spz->info->id, mode);
89 }
90 
91 static const struct powercap_zone_ops zone_ops = {
92 	.get_max_power_range_uw = scmi_powercap_get_max_power_range_uw,
93 	.get_power_uw = scmi_powercap_get_power_uw,
94 	.release = scmi_powercap_zone_release,
95 	.set_enable = scmi_powercap_zone_enable_set,
96 	.get_enable = scmi_powercap_zone_enable_get,
97 };
98 
99 static void scmi_powercap_normalize_cap(const struct scmi_powercap_zone *spz,
100 					u64 power_limit_uw, u32 *norm)
101 {
102 	bool scale_mw = spz->info->powercap_scale_mw;
103 	u64 val;
104 
105 	val = scale_mw ? DIV_ROUND_UP_ULL(power_limit_uw, 1000) : power_limit_uw;
106 	/*
107 	 * This cast is lossless since here @req_power is certain to be within
108 	 * the range [min_power_cap, max_power_cap] whose bounds are assured to
109 	 * be two unsigned 32bits quantities.
110 	 */
111 	*norm = clamp_t(u32, val, spz->info->min_power_cap,
112 			spz->info->max_power_cap);
113 	*norm = rounddown(*norm, spz->info->power_cap_step);
114 
115 	val = (scale_mw) ? *norm * 1000 : *norm;
116 	if (power_limit_uw != val)
117 		dev_dbg(spz->dev,
118 			"Normalized %s:CAP - requested:%llu - normalized:%llu\n",
119 			spz->info->name, power_limit_uw, val);
120 }
121 
122 static int scmi_powercap_set_power_limit_uw(struct powercap_zone *pz, int cid,
123 					    u64 power_uw)
124 {
125 	struct scmi_powercap_zone *spz = to_scmi_powercap_zone(pz);
126 	u32 norm_power;
127 
128 	if (!spz->info->powercap_cap_config)
129 		return -EINVAL;
130 
131 	scmi_powercap_normalize_cap(spz, power_uw, &norm_power);
132 
133 	return powercap_ops->cap_set(spz->ph, spz->info->id, norm_power, false);
134 }
135 
136 static int scmi_powercap_get_power_limit_uw(struct powercap_zone *pz, int cid,
137 					    u64 *power_limit_uw)
138 {
139 	struct scmi_powercap_zone *spz = to_scmi_powercap_zone(pz);
140 	u32 power;
141 	int ret;
142 
143 	ret = powercap_ops->cap_get(spz->ph, spz->info->id, &power);
144 	if (ret)
145 		return ret;
146 
147 	*power_limit_uw = power;
148 	if (spz->info->powercap_scale_mw)
149 		*power_limit_uw *= 1000;
150 
151 	return 0;
152 }
153 
154 static void scmi_powercap_normalize_time(const struct scmi_powercap_zone *spz,
155 					 u64 time_us, u32 *norm)
156 {
157 	/*
158 	 * This cast is lossless since here @time_us is certain to be within the
159 	 * range [min_pai, max_pai] whose bounds are assured to be two unsigned
160 	 * 32bits quantities.
161 	 */
162 	*norm = clamp_t(u32, time_us, spz->info->min_pai, spz->info->max_pai);
163 	*norm = rounddown(*norm, spz->info->pai_step);
164 
165 	if (time_us != *norm)
166 		dev_dbg(spz->dev,
167 			"Normalized %s:PAI - requested:%llu - normalized:%u\n",
168 			spz->info->name, time_us, *norm);
169 }
170 
171 static int scmi_powercap_set_time_window_us(struct powercap_zone *pz, int cid,
172 					    u64 time_window_us)
173 {
174 	struct scmi_powercap_zone *spz = to_scmi_powercap_zone(pz);
175 	u32 norm_pai;
176 
177 	if (!spz->info->powercap_pai_config)
178 		return -EINVAL;
179 
180 	scmi_powercap_normalize_time(spz, time_window_us, &norm_pai);
181 
182 	return powercap_ops->pai_set(spz->ph, spz->info->id, norm_pai);
183 }
184 
185 static int scmi_powercap_get_time_window_us(struct powercap_zone *pz, int cid,
186 					    u64 *time_window_us)
187 {
188 	struct scmi_powercap_zone *spz = to_scmi_powercap_zone(pz);
189 	int ret;
190 	u32 pai;
191 
192 	ret = powercap_ops->pai_get(spz->ph, spz->info->id, &pai);
193 	if (ret)
194 		return ret;
195 
196 	*time_window_us = pai;
197 
198 	return 0;
199 }
200 
201 static int scmi_powercap_get_max_power_uw(struct powercap_zone *pz, int cid,
202 					  u64 *max_power_uw)
203 {
204 	struct scmi_powercap_zone *spz = to_scmi_powercap_zone(pz);
205 
206 	*max_power_uw = spz->info->max_power_cap;
207 	if (spz->info->powercap_scale_mw)
208 		*max_power_uw *= 1000;
209 
210 	return 0;
211 }
212 
213 static int scmi_powercap_get_min_power_uw(struct powercap_zone *pz, int cid,
214 					  u64 *min_power_uw)
215 {
216 	struct scmi_powercap_zone *spz = to_scmi_powercap_zone(pz);
217 
218 	*min_power_uw = spz->info->min_power_cap;
219 	if (spz->info->powercap_scale_mw)
220 		*min_power_uw *= 1000;
221 
222 	return 0;
223 }
224 
225 static int scmi_powercap_get_max_time_window_us(struct powercap_zone *pz,
226 						int cid, u64 *time_window_us)
227 {
228 	struct scmi_powercap_zone *spz = to_scmi_powercap_zone(pz);
229 
230 	*time_window_us = spz->info->max_pai;
231 
232 	return 0;
233 }
234 
235 static int scmi_powercap_get_min_time_window_us(struct powercap_zone *pz,
236 						int cid, u64 *time_window_us)
237 {
238 	struct scmi_powercap_zone *spz = to_scmi_powercap_zone(pz);
239 
240 	*time_window_us = (u64)spz->info->min_pai;
241 
242 	return 0;
243 }
244 
245 static const char *scmi_powercap_get_name(struct powercap_zone *pz, int cid)
246 {
247 	return "SCMI power-cap";
248 }
249 
250 static const struct powercap_zone_constraint_ops constraint_ops  = {
251 	.set_power_limit_uw = scmi_powercap_set_power_limit_uw,
252 	.get_power_limit_uw = scmi_powercap_get_power_limit_uw,
253 	.set_time_window_us = scmi_powercap_set_time_window_us,
254 	.get_time_window_us = scmi_powercap_get_time_window_us,
255 	.get_max_power_uw = scmi_powercap_get_max_power_uw,
256 	.get_min_power_uw = scmi_powercap_get_min_power_uw,
257 	.get_max_time_window_us = scmi_powercap_get_max_time_window_us,
258 	.get_min_time_window_us = scmi_powercap_get_min_time_window_us,
259 	.get_name = scmi_powercap_get_name,
260 };
261 
262 static void scmi_powercap_unregister_all_zones(struct scmi_powercap_root *pr)
263 {
264 	int i;
265 
266 	/* Un-register children zones first starting from the leaves */
267 	for (i = pr->num_zones - 1; i >= 0; i--) {
268 		if (!list_empty(&pr->registered_zones[i])) {
269 			struct scmi_powercap_zone *spz;
270 
271 			list_for_each_entry(spz, &pr->registered_zones[i], node)
272 				powercap_unregister_zone(scmi_top_pcntrl,
273 							 &spz->zone);
274 		}
275 	}
276 }
277 
278 static inline unsigned int
279 scmi_powercap_get_zone_height(struct scmi_powercap_zone *spz)
280 {
281 	if (spz->info->parent_id == SCMI_POWERCAP_ROOT_ZONE_ID)
282 		return 0;
283 
284 	return spz->spzones[spz->info->parent_id].height + 1;
285 }
286 
287 static inline struct scmi_powercap_zone *
288 scmi_powercap_get_parent_zone(struct scmi_powercap_zone *spz)
289 {
290 	if (spz->info->parent_id == SCMI_POWERCAP_ROOT_ZONE_ID)
291 		return NULL;
292 
293 	return &spz->spzones[spz->info->parent_id];
294 }
295 
296 static int scmi_powercap_register_zone(struct scmi_powercap_root *pr,
297 				       struct scmi_powercap_zone *spz,
298 				       struct scmi_powercap_zone *parent)
299 {
300 	int ret = 0;
301 	struct powercap_zone *z;
302 
303 	if (spz->invalid) {
304 		list_del(&spz->node);
305 		return -EINVAL;
306 	}
307 
308 	z = powercap_register_zone(&spz->zone, scmi_top_pcntrl, spz->info->name,
309 				   parent ? &parent->zone : NULL,
310 				   &zone_ops, 1, &constraint_ops);
311 	if (!IS_ERR(z)) {
312 		spz->height = scmi_powercap_get_zone_height(spz);
313 		spz->registered = true;
314 		list_move(&spz->node, &pr->registered_zones[spz->height]);
315 		dev_dbg(spz->dev, "Registered node %s - parent %s - height:%d\n",
316 			spz->info->name, parent ? parent->info->name : "ROOT",
317 			spz->height);
318 	} else {
319 		list_del(&spz->node);
320 		ret = PTR_ERR(z);
321 		dev_err(spz->dev,
322 			"Error registering node:%s - parent:%s - h:%d - ret:%d\n",
323 			spz->info->name,
324 			parent ? parent->info->name : "ROOT",
325 			spz->height, ret);
326 	}
327 
328 	return ret;
329 }
330 
331 /**
332  * scmi_zones_register- Register SCMI powercap zones starting from parent zones
333  *
334  * @dev: A reference to the SCMI device
335  * @pr: A reference to the root powercap zones descriptors
336  *
337  * When registering SCMI powercap zones with the powercap framework we should
338  * take care to always register zones starting from the root ones and to
339  * deregister starting from the leaves.
340  *
341  * Unfortunately we cannot assume that the array of available SCMI powercap
342  * zones provided by the SCMI platform firmware is built to comply with such
343  * requirement.
344  *
345  * This function, given the set of SCMI powercap zones to register, takes care
346  * to walk the SCMI powercap zones trees up to the root registering any
347  * unregistered parent zone before registering the child zones; at the same
348  * time each registered-zone height in such a tree is accounted for and each
349  * zone, once registered, is stored in the @registered_zones array that is
350  * indexed by zone height: this way will be trivial, at unregister time, to walk
351  * the @registered_zones array backward and unregister all the zones starting
352  * from the leaves, removing children zones before parents.
353  *
354  * While doing this, we prune away any zone marked as invalid (like the ones
355  * sporting an SCMI abstract power scale) as long as they are positioned as
356  * leaves in the SCMI powercap zones hierarchy: any non-leaf invalid zone causes
357  * the entire process to fail since we cannot assume the correctness of an SCMI
358  * powercap zones hierarchy if some of the internal nodes are missing.
359  *
360  * Note that the array of SCMI powercap zones as returned by the SCMI platform
361  * is known to be sane, i.e. zones relationships have been validated at the
362  * protocol layer.
363  *
364  * Return: 0 on Success
365  */
366 static int scmi_zones_register(struct device *dev,
367 			       struct scmi_powercap_root *pr)
368 {
369 	int ret = 0;
370 	unsigned int sp = 0, reg_zones = 0;
371 	struct scmi_powercap_zone *spz, **zones_stack;
372 
373 	zones_stack = kcalloc(pr->num_zones, sizeof(spz), GFP_KERNEL);
374 	if (!zones_stack)
375 		return -ENOMEM;
376 
377 	spz = list_first_entry_or_null(&pr->scmi_zones,
378 				       struct scmi_powercap_zone, node);
379 	while (spz) {
380 		struct scmi_powercap_zone *parent;
381 
382 		parent = scmi_powercap_get_parent_zone(spz);
383 		if (parent && !parent->registered) {
384 			zones_stack[sp++] = spz;
385 			spz = parent;
386 		} else {
387 			ret = scmi_powercap_register_zone(pr, spz, parent);
388 			if (!ret) {
389 				reg_zones++;
390 			} else if (sp) {
391 				/* Failed to register a non-leaf zone.
392 				 * Bail-out.
393 				 */
394 				dev_err(dev,
395 					"Failed to register non-leaf zone - ret:%d\n",
396 					ret);
397 				scmi_powercap_unregister_all_zones(pr);
398 				reg_zones = 0;
399 				goto out;
400 			}
401 			/* Pick next zone to process */
402 			if (sp)
403 				spz = zones_stack[--sp];
404 			else
405 				spz = list_first_entry_or_null(&pr->scmi_zones,
406 							       struct scmi_powercap_zone,
407 							       node);
408 		}
409 	}
410 
411 out:
412 	kfree(zones_stack);
413 	dev_info(dev, "Registered %d SCMI Powercap domains !\n", reg_zones);
414 
415 	return ret;
416 }
417 
418 static int scmi_powercap_probe(struct scmi_device *sdev)
419 {
420 	int ret, i;
421 	struct scmi_powercap_root *pr;
422 	struct scmi_powercap_zone *spz;
423 	struct scmi_protocol_handle *ph;
424 	struct device *dev = &sdev->dev;
425 
426 	if (!sdev->handle)
427 		return -ENODEV;
428 
429 	powercap_ops = sdev->handle->devm_protocol_get(sdev,
430 						       SCMI_PROTOCOL_POWERCAP,
431 						       &ph);
432 	if (IS_ERR(powercap_ops))
433 		return PTR_ERR(powercap_ops);
434 
435 	pr = devm_kzalloc(dev, sizeof(*pr), GFP_KERNEL);
436 	if (!pr)
437 		return -ENOMEM;
438 
439 	ret = powercap_ops->num_domains_get(ph);
440 	if (ret < 0) {
441 		dev_err(dev, "number of powercap domains not found\n");
442 		return ret;
443 	}
444 	pr->num_zones = ret;
445 
446 	pr->spzones = devm_kcalloc(dev, pr->num_zones,
447 				   sizeof(*pr->spzones), GFP_KERNEL);
448 	if (!pr->spzones)
449 		return -ENOMEM;
450 
451 	/* Allocate for worst possible scenario of maximum tree height. */
452 	pr->registered_zones = devm_kcalloc(dev, pr->num_zones,
453 					    sizeof(*pr->registered_zones),
454 					    GFP_KERNEL);
455 	if (!pr->registered_zones)
456 		return -ENOMEM;
457 
458 	INIT_LIST_HEAD(&pr->scmi_zones);
459 
460 	for (i = 0, spz = pr->spzones; i < pr->num_zones; i++, spz++) {
461 		/*
462 		 * Powercap domains are validate by the protocol layer, i.e.
463 		 * when only non-NULL domains are returned here, whose
464 		 * parent_id is assured to point to another valid domain.
465 		 */
466 		spz->info = powercap_ops->info_get(ph, i);
467 
468 		spz->dev = dev;
469 		spz->ph = ph;
470 		spz->spzones = pr->spzones;
471 		INIT_LIST_HEAD(&spz->node);
472 		INIT_LIST_HEAD(&pr->registered_zones[i]);
473 
474 		list_add_tail(&spz->node, &pr->scmi_zones);
475 		/*
476 		 * Forcibly skip powercap domains using an abstract scale.
477 		 * Note that only leaves domains can be skipped, so this could
478 		 * lead later to a global failure.
479 		 */
480 		if (!spz->info->powercap_scale_uw &&
481 		    !spz->info->powercap_scale_mw) {
482 			dev_warn(dev,
483 				 "Abstract power scale not supported. Skip %s.\n",
484 				 spz->info->name);
485 			spz->invalid = true;
486 			continue;
487 		}
488 	}
489 
490 	/*
491 	 * Scan array of retrieved SCMI powercap domains and register them
492 	 * recursively starting from the root domains.
493 	 */
494 	ret = scmi_zones_register(dev, pr);
495 	if (ret)
496 		return ret;
497 
498 	dev_set_drvdata(dev, pr);
499 
500 	return ret;
501 }
502 
503 static void scmi_powercap_remove(struct scmi_device *sdev)
504 {
505 	struct device *dev = &sdev->dev;
506 	struct scmi_powercap_root *pr = dev_get_drvdata(dev);
507 
508 	scmi_powercap_unregister_all_zones(pr);
509 }
510 
511 static const struct scmi_device_id scmi_id_table[] = {
512 	{ SCMI_PROTOCOL_POWERCAP, "powercap" },
513 	{ },
514 };
515 MODULE_DEVICE_TABLE(scmi, scmi_id_table);
516 
517 static struct scmi_driver scmi_powercap_driver = {
518 	.name = "scmi-powercap",
519 	.probe = scmi_powercap_probe,
520 	.remove = scmi_powercap_remove,
521 	.id_table = scmi_id_table,
522 };
523 
524 static int __init scmi_powercap_init(void)
525 {
526 	int ret;
527 
528 	scmi_top_pcntrl = powercap_register_control_type(NULL, "arm-scmi", NULL);
529 	if (IS_ERR(scmi_top_pcntrl))
530 		return PTR_ERR(scmi_top_pcntrl);
531 
532 	ret = scmi_register(&scmi_powercap_driver);
533 	if (ret)
534 		powercap_unregister_control_type(scmi_top_pcntrl);
535 
536 	return ret;
537 }
538 module_init(scmi_powercap_init);
539 
540 static void __exit scmi_powercap_exit(void)
541 {
542 	scmi_unregister(&scmi_powercap_driver);
543 
544 	powercap_unregister_control_type(scmi_top_pcntrl);
545 }
546 module_exit(scmi_powercap_exit);
547 
548 MODULE_AUTHOR("Cristian Marussi <cristian.marussi@arm.com>");
549 MODULE_DESCRIPTION("ARM SCMI Powercap driver");
550 MODULE_LICENSE("GPL");
551