| /* |
| * Google LWIS Dynamic Power Managerment |
| * |
| * Copyright (c) 2020 Google, LLC |
| * |
| * This program is free software; you can redistribute it and/or modify |
| * it under the terms of the GNU General Public License version 2 as |
| * published by the Free Software Foundation. |
| */ |
| |
| #define pr_fmt(fmt) KBUILD_MODNAME "-dpm: " fmt |
| |
| #include "lwis_device_dpm.h" |
| |
| #include <linux/clk.h> |
| #include <linux/slab.h> |
| |
| #include "lwis_commands.h" |
| #include "lwis_init.h" |
| #include "lwis_platform.h" |
| |
| #define LWIS_DRIVER_NAME "lwis-dpm" |
| |
| static struct lwis_device_subclass_operations dpm_vops = { |
| .register_io = NULL, |
| .register_io_barrier = NULL, |
| .device_enable = NULL, |
| .device_disable = NULL, |
| .event_enable = NULL, |
| .event_flags_updated = NULL, |
| .close = NULL, |
| }; |
| |
| static struct lwis_event_subscribe_operations dpm_subscribe_ops = { |
| .subscribe_event = NULL, |
| .unsubscribe_event = NULL, |
| .notify_event_subscriber = NULL, |
| .release = NULL, |
| }; |
| |
| /* |
| * lwis_dpm_update_qos: update qos requirement for lwis device. |
| */ |
| int lwis_dpm_update_qos(struct lwis_device *lwis_dev, struct lwis_qos_setting *qos_setting) |
| { |
| int ret = 0; |
| int64_t peak_bw = 0; |
| int64_t read_bw = 0; |
| int64_t write_bw = 0; |
| int64_t rt_bw = 0; |
| struct lwis_device *target_dev = lwis_find_dev_by_id(qos_setting->device_id); |
| if (!target_dev) { |
| dev_err(lwis_dev->dev, "Can't find device by id: %d\n", qos_setting->device_id); |
| return -ENOENT; |
| } |
| |
| /* b/190270885 : We see some ramdump issues due to dpm qos updates |
| * when device is disabled. We might disallow to update qos on this case. |
| */ |
| if (target_dev->enabled == 0 && target_dev->type != DEVICE_TYPE_DPM) { |
| dev_warn(target_dev->dev, "%s disabled, no need to update qos\n", target_dev->name); |
| return -EPERM; |
| } |
| |
| switch (qos_setting->clock_family) { |
| case CLOCK_FAMILY_MIF: |
| case CLOCK_FAMILY_INT: |
| if (qos_setting->frequency_hz >= 0 && target_dev->type == DEVICE_TYPE_DPM) { |
| /* vote to qos if frequency is specified. The vote only available for dpm |
| * device |
| */ |
| ret = lwis_platform_update_qos(lwis_dev, |
| (int)(qos_setting->frequency_hz / 1000), |
| qos_setting->clock_family); |
| if (ret) { |
| dev_err(lwis_dev->dev, |
| "Failed to vote to qos for clock family %d\n", |
| qos_setting->clock_family); |
| } |
| } else { |
| read_bw = qos_setting->read_bw; |
| write_bw = qos_setting->write_bw; |
| peak_bw = (qos_setting->peak_bw > 0) ? |
| qos_setting->peak_bw : |
| ((read_bw > write_bw) ? read_bw : write_bw) / 4; |
| rt_bw = (qos_setting->rt_bw > 0) ? qos_setting->rt_bw : 0; |
| ret = lwis_platform_update_bts(target_dev, peak_bw, read_bw, write_bw, rt_bw); |
| if (ret < 0) { |
| dev_err(lwis_dev->dev, "Failed to update bandwidth to bts, ret: %d\n", ret); |
| } |
| } |
| break; |
| case CLOCK_FAMILY_TNR: |
| case CLOCK_FAMILY_CAM: |
| case CLOCK_FAMILY_INTCAM: |
| /* convert value to KHz */ |
| ret = lwis_platform_update_qos(target_dev, (int)(qos_setting->frequency_hz / 1000), |
| qos_setting->clock_family); |
| if (ret) { |
| dev_err(lwis_dev->dev, |
| "Failed to apply core clock requirement for %s, ret: %d\n", |
| target_dev->name, ret); |
| } |
| break; |
| default: |
| dev_err(lwis_dev->dev, "Invalid clock family %d\n", qos_setting->clock_family); |
| ret = -EINVAL; |
| } |
| |
| return ret; |
| } |
| |
| /* |
| * lwis_dpm_update_clock: update specific clock settings to lwis device. |
| */ |
| int lwis_dpm_update_clock(struct lwis_device *lwis_dev, struct lwis_clk_setting *clk_settings, |
| size_t num_settings) |
| { |
| int ret = 0, i, clk_index; |
| uint32_t old_clk; |
| |
| if (!lwis_dev->clocks) { |
| dev_err(lwis_dev->dev, "%s has no clocks\n", lwis_dev->name); |
| ret = -EINVAL; |
| goto out; |
| } |
| |
| for (i = 0; i < num_settings; ++i) { |
| clk_index = clk_settings[i].clk_index; |
| if (clk_index < 0 || clk_index >= lwis_dev->clocks->count) { |
| dev_err(lwis_dev->dev, "%s clk index %d is invalid\n", lwis_dev->name, |
| clk_index); |
| ret = -EINVAL; |
| goto out; |
| } |
| |
| if (IS_ERR(lwis_dev->clocks->clk[clk_index].clk)) { |
| dev_err(lwis_dev->dev, "%s clk is invalid\n", lwis_dev->name); |
| ret = -EINVAL; |
| goto out; |
| } |
| |
| old_clk = clk_get_rate(lwis_dev->clocks->clk[clk_index].clk); |
| if (old_clk == clk_settings[i].frequency) |
| continue; |
| |
| ret = clk_set_rate(lwis_dev->clocks->clk[clk_index].clk, clk_settings[i].frequency); |
| if (ret) { |
| dev_err(lwis_dev->dev, "Error updating clock %s freq: %u\n", |
| lwis_dev->clocks->clk[clk_index].name, clk_settings[i].frequency); |
| goto out; |
| } |
| |
| dev_info(lwis_dev->dev, "Update %s freq from %u to %u, clock read back: %lu\n", |
| lwis_dev->clocks->clk[clk_index].name, old_clk, clk_settings[i].frequency, |
| clk_get_rate(lwis_dev->clocks->clk[clk_index].clk)); |
| } |
| out: |
| return ret; |
| } |
| |
| static int lwis_dpm_device_probe(struct platform_device *plat_dev) |
| { |
| int ret = 0; |
| struct lwis_dpm_device *dpm_dev; |
| struct device *dev = &plat_dev->dev; |
| |
| /* Allocate top device specific data construct */ |
| dpm_dev = devm_kzalloc(dev, sizeof(struct lwis_dpm_device), GFP_KERNEL); |
| if (!dpm_dev) { |
| dev_err(dev, "Failed to allocate dpm device structure\n"); |
| return -ENOMEM; |
| } |
| |
| dpm_dev->base_dev.type = DEVICE_TYPE_DPM; |
| dpm_dev->base_dev.vops = dpm_vops; |
| dpm_dev->base_dev.subscribe_ops = dpm_subscribe_ops; |
| |
| /* Call the base device probe function */ |
| ret = lwis_base_probe((struct lwis_device *)dpm_dev, plat_dev); |
| if (ret) { |
| dev_err(dev, "Error in lwis base probe, ret: %d\n", ret); |
| } |
| |
| return ret; |
| } |
| |
| #ifdef CONFIG_OF |
| static const struct of_device_id lwis_id_match[] = { |
| { .compatible = LWIS_DPM_DEVICE_COMPAT }, |
| {}, |
| }; |
| // MODULE_DEVICE_TABLE(of, lwis_id_match); |
| |
| static struct platform_driver lwis_driver = { |
| .probe = lwis_dpm_device_probe, |
| .driver = { |
| .name = LWIS_DRIVER_NAME, |
| .owner = THIS_MODULE, |
| .of_match_table = lwis_id_match, |
| }, |
| }; |
| |
| #else /* CONFIG_OF not defined */ |
| static struct platform_device_id lwis_driver_id[] = { |
| { |
| .name = LWIS_DRIVER_NAME, |
| .driver_data = 0, |
| }, |
| {}, |
| }; |
| MODULE_DEVICE_TABLE(platform, lwis_driver_id); |
| |
| static struct platform_driver lwis_driver = { .probe = lwis_dpm_device_probe, |
| .id_table = lwis_driver_id, |
| .driver = { |
| .name = LWIS_DRIVER_NAME, |
| .owner = THIS_MODULE, |
| } }; |
| #endif /* CONFIG_OF */ |
| |
| /* |
| * lwis_dpm_device_init: Init function that will be called by the kernel |
| * initialization routines. |
| */ |
| int __init lwis_dpm_device_init(void) |
| { |
| int ret = 0; |
| |
| pr_info("DPM device initialization\n"); |
| |
| ret = platform_driver_register(&lwis_driver); |
| if (ret) |
| pr_err("platform_driver_register failed: %d\n", ret); |
| |
| return ret; |
| } |
| |
| int lwis_dpm_device_deinit(void) |
| { |
| platform_driver_unregister(&lwis_driver); |
| return 0; |
| } |
| |
| uint32_t lwis_dpm_read_clock(struct lwis_device *lwis_dev) |
| { |
| uint32_t clock = 0; |
| |
| if (!lwis_dev->clocks) { |
| dev_err(lwis_dev->dev, "%s clock not defined", lwis_dev->name); |
| return -ENODEV; |
| } |
| clock = clk_get_rate(lwis_dev->clocks->clk[0].clk); |
| return clock; |
| } |