static void drv_write(struct drv_cmd *cmd)
{
if (cpumask_equal(cmd->mask, cpumask_of(smp_processor_id())))
do_drv_write((void *)cmd);
else
on_selected_cpus(cmd->mask, do_drv_write, cmd, 1);
}
static void drv_write(struct drv_cmd *cmd)
{
int this_cpu;
this_cpu = get_cpu();
if (cpumask_test_cpu(this_cpu, cmd->mask))
do_drv_write(cmd);
smp_call_function_many(cmd->mask, do_drv_write, cmd, 1);
put_cpu();
}
static u32 drv_read(struct acpi_cpufreq_data *data, const struct cpumask *mask)
{
struct acpi_processor_performance *perf = to_perf_data(data);
struct drv_cmd cmd = {
.reg = &perf->control_register,
.func.read = data->cpu_freq_read,
};
int err;
err = smp_call_function_any(mask, do_drv_read, &cmd, 1);
WARN_ON_ONCE(err); /* smp_call_function_any() was buggy? */
return cmd.val;
}
/* Called via smp_call_function_many(), on the target CPUs */
static void do_drv_write(void *_cmd)
{
struct drv_cmd *cmd = _cmd;
cmd->func.write(cmd->reg, cmd->val);
}
static void drv_write(struct acpi_cpufreq_data *data,
const struct cpumask *mask, u32 val)
{
struct acpi_processor_performance *perf = to_perf_data(data);
struct drv_cmd cmd = {
.reg = &perf->control_register,
.val = val,
.func.write = data->cpu_freq_write,
};
int this_cpu;
this_cpu = get_cpu();
if (cpumask_test_cpu(this_cpu, mask))
do_drv_write(&cmd);
smp_call_function_many(mask, do_drv_write, &cmd, 1);
put_cpu();
}
static u32 get_cur_val(const struct cpumask *mask, struct acpi_cpufreq_data *data)
{
u32 val;
if (unlikely(cpumask_empty(mask)))
return 0;
val = drv_read(data, mask);
pr_debug("get_cur_val = %u\n", val);
return val;
}
static unsigned int get_cur_freq_on_cpu(unsigned int cpu)
{
struct acpi_cpufreq_data *data;
struct cpufreq_policy *policy;
unsigned int freq;
unsigned int cached_freq;
pr_debug("get_cur_freq_on_cpu (%d)\n", cpu);
policy = cpufreq_cpu_get_raw(cpu);
if (unlikely(!policy))
return 0;
data = policy->driver_data;
if (unlikely(!data || !policy->freq_table))
return 0;
cached_freq = policy->freq_table[to_perf_data(data)->state].frequency;
freq = extract_freq(policy, get_cur_val(cpumask_of(cpu), data));
if (freq != cached_freq) {
/*
* The dreaded BIOS frequency change behind our back.
* Force set the frequency on next target call.
*/
data->resume = 1;
}
pr_debug("cur freq = %u\n", freq);
return freq;
}
static unsigned int check_freqs(struct cpufreq_policy *policy,
const struct cpumask *mask, unsigned int freq)
{
struct acpi_cpufreq_data *data = policy->driver_data;
unsigned int cur_freq;
unsigned int i;
for (i = 0; i < 100; i++) {
cur_freq = extract_freq(policy, get_cur_val(mask, data));
if (cur_freq == freq)
return 1;
udelay(10);
}
return 0;
}
static int acpi_cpufreq_target(struct cpufreq_policy *policy,
unsigned int index)
{
struct acpi_cpufreq_data *data = policy->driver_data;
struct acpi_processor_performance *perf;
const struct cpumask *mask;
unsigned int next_perf_state = 0; /* Index into perf table */
int result = 0;
if (unlikely(!data)) {
return -ENODEV;
}
perf = to_perf_data(data);
next_perf_state = policy->freq_table[index].driver_data;
if (perf->state == next_perf_state) {
if (unlikely(data->resume)) {
pr_debug("Called after resume, resetting to P%d\n",
next_perf_state);
data->resume = 0;
} else {
pr_debug("Already at target state (P%d)\n",
next_perf_state);
return 0;
}
}
/*
* The core won't allow CPUs to go away until the governor has been
* stopped, so we can rely on the stability of policy->cpus.
*/
mask = policy->shared_type == CPUFREQ_SHARED_TYPE_ANY ?
cpumask_of(policy->cpu) : policy->cpus;
drv_write(data, mask, perf->states[next_perf_state].control);
if (acpi_pstate_strict) {
if (!check_freqs(policy, mask,
policy->freq_table[index].frequency)) {
pr_debug("acpi_cpufreq_target failed (%d)\n",
policy->cpu);
result = -EAGAIN;
}
}
if (!result)
perf->state = next_perf_state;
return result;
}
unsigned int acpi_cpufreq_fast_switch(struct cpufreq_policy *policy,
unsigned int target_freq)
{
struct acpi_cpufreq_data *data = policy->driver_data;
struct acpi_processor_performance *perf;
struct cpufreq_frequency_table *entry;
unsigned int next_perf_state, next_freq, index;
/*
* Find the closest frequency above target_freq.
*/
if (policy->cached_target_freq == target_freq)
index = policy->cached_resolved_idx;
else
index = cpufreq_table_find_index_dl(policy, target_freq);
entry = &policy->freq_table[index];
next_freq = entry->frequency;
next_perf_state = entry->driver_data;
perf = to_perf_data(data);
if (perf->state == next_perf_state) {
if (unlikely(data->resume))
data->resume = 0;
else
return next_freq;
}
data->cpu_freq_write(&perf->control_register,
perf->states[next_perf_state].control);
perf->state = next_perf_state;
return next_freq;
}
static unsigned long
acpi_cpufreq_guess_freq(struct acpi_cpufreq_data *data, unsigned int cpu)
{
struct acpi_processor_performance *perf;
perf = to_perf_data(data);
if (cpu_khz) {
/* search the closest match to cpu_khz */
unsigned int i;
unsigned long freq;
unsigned long freqn = perf->states[0].core_frequency * 1000;
for (i = 0; i < (perf->state_count-1); i++) {
freq = freqn;
freqn = perf->states[i+1].core_frequency * 1000;
if ((2 * cpu_khz) > (freqn + freq)) {
perf->state = i;
return freq;
}
}
perf->state = perf->state_count-1;
return freqn;
} else {
/* assume CPU is at P0... */
perf->state = 0;
return perf->states[0].core_frequency * 1000;
}
}
static void free_acpi_perf_data(void)
{
unsigned int i;
/* Freeing a NULL pointer is OK, and alloc_percpu zeroes. */
for_each_possible_cpu(i)
free_cpumask_var(per_cpu_ptr(acpi_perf_data, i)
->shared_cpu_map);
free_percpu(acpi_perf_data);
}
