speedstep-centrino.c

来自「Linux Kernel 2.6.9 for OMAP1710」· C语言 代码 · 共 679 行 · 第 1/2 页

	unsigned long			cur_freq;
	int				result = 0, i;

	/* _PDC settings */
        arg0.buffer.length = 12;
        arg0.buffer.pointer = (u8 *) arg0_buf;
        arg0_buf[0] = ACPI_PDC_REVISION_ID;
        arg0_buf[1] = 1;
        arg0_buf[2] = ACPI_PDC_EST_CAPABILITY_SMP | ACPI_PDC_EST_CAPABILITY_MSR;

	p.pdc = &arg_list;

	/* register with ACPI core */
        if (acpi_processor_register_performance(&p, policy->cpu))
                return -EIO;

	/* verify the acpi_data */
	if (p.state_count <= 1) {
                printk(KERN_DEBUG "No P-States\n");
                result = -ENODEV;
                goto err_unreg;
	}

	if ((p.control_register.space_id != ACPI_ADR_SPACE_FIXED_HARDWARE) ||
	    (p.status_register.space_id != ACPI_ADR_SPACE_FIXED_HARDWARE)) {
		printk(KERN_DEBUG "Invalid control/status registers\n");
		result = -EIO;
		goto err_unreg;
	}

	for (i=0; i<p.state_count; i++) {
		if (p.states[i].control != p.states[i].status) {
			printk(KERN_DEBUG "Different control and status values\n");
			result = -EINVAL;
			goto err_unreg;
		}

		if (!p.states[i].core_frequency) {
			printk(KERN_DEBUG "Zero core frequency\n");
			result = -EINVAL;
			goto err_unreg;
		}

		if (p.states[i].core_frequency > p.states[0].core_frequency) {
			printk(KERN_DEBUG "P%u has larger frequency than P0, skipping\n", i);
			p.states[i].core_frequency = 0;
			continue;
		}
	}

	centrino_model = kmalloc(sizeof(struct cpu_model), GFP_KERNEL);
	if (!centrino_model) {
		result = -ENOMEM;
		goto err_unreg;
	}
	memset(centrino_model, 0, sizeof(struct cpu_model));

	centrino_model->model_name=NULL;
	centrino_model->max_freq = p.states[0].core_frequency * 1000;
	centrino_model->op_points =  kmalloc(sizeof(struct cpufreq_frequency_table) *
					     (p.state_count + 1), GFP_KERNEL);
        if (!centrino_model->op_points) {
                result = -ENOMEM;
                goto err_kfree;
        }

        for (i=0; i<p.state_count; i++) {
		centrino_model->op_points[i].index = p.states[i].control;
		centrino_model->op_points[i].frequency = p.states[i].core_frequency * 1000;
	}
	centrino_model->op_points[p.state_count].frequency = CPUFREQ_TABLE_END;

	cur_freq = get_cur_freq(policy->cpu);

	for (i=0; i<p.state_count; i++) {
		if (extract_clock(centrino_model->op_points[i].index) !=
		    (centrino_model->op_points[i].frequency)) {
			printk(KERN_DEBUG "Invalid encoded frequency\n");
			result = -EINVAL;
			goto err_kfree_all;
		}

		if (cur_freq == centrino_model->op_points[i].frequency)
			p.state = i;
		if (!p.states[i].core_frequency)
			centrino_model->op_points[i].frequency = CPUFREQ_ENTRY_INVALID;
	}

	return 0;

 err_kfree_all:
	kfree(centrino_model->op_points);
 err_kfree:
	kfree(centrino_model);
 err_unreg:
	acpi_processor_unregister_performance(&p, policy->cpu);
	return (result);
}
#else
static inline int centrino_cpu_init_acpi(struct cpufreq_policy *policy) { return -ENODEV; }
#endif

static int centrino_cpu_init(struct cpufreq_policy *policy)
{
	struct cpuinfo_x86 *cpu = &cpu_data[policy->cpu];
	unsigned freq;
	unsigned l, h;
	int ret;
	int i;

	/* Only Intel makes Enhanced Speedstep-capable CPUs */
	if (cpu->x86_vendor != X86_VENDOR_INTEL || !cpu_has(cpu, X86_FEATURE_EST))
		return -ENODEV;

	for (i = 0; i < N_IDS; i++)
		if (centrino_verify_cpu_id(cpu, &cpu_ids[i]))
			break;

	if (i != N_IDS)
		centrino_cpu = 1;

	if (centrino_cpu_init_acpi(policy)) {
		if (policy->cpu != 0)
			return -ENODEV;

		if (!centrino_cpu) {
			printk(KERN_INFO PFX "found unsupported CPU with "
			"Enhanced SpeedStep: send /proc/cpuinfo to "
			MAINTAINER "\n");
			return -ENODEV;
		}

		if (centrino_cpu_init_table(policy)) {
			return -ENODEV;
		}
	}

	/* Check to see if Enhanced SpeedStep is enabled, and try to
	   enable it if not. */
	rdmsr(MSR_IA32_MISC_ENABLE, l, h);

	if (!(l & (1<<16))) {
		l |= (1<<16);
		wrmsr(MSR_IA32_MISC_ENABLE, l, h);

		/* check to see if it stuck */
		rdmsr(MSR_IA32_MISC_ENABLE, l, h);
		if (!(l & (1<<16))) {
			printk(KERN_INFO PFX "couldn't enable Enhanced SpeedStep\n");
			return -ENODEV;
		}
	}

	freq = get_cur_freq(policy->cpu);

	policy->governor = CPUFREQ_DEFAULT_GOVERNOR;
	policy->cpuinfo.transition_latency = 10000; /* 10uS transition latency */
	policy->cur = freq;

	dprintk(KERN_INFO PFX "centrino_cpu_init: policy=%d cur=%dkHz\n",
		policy->policy, policy->cur);

	ret = cpufreq_frequency_table_cpuinfo(policy, centrino_model->op_points);
	if (ret)
		return (ret);

	cpufreq_frequency_table_get_attr(centrino_model->op_points, policy->cpu);

	return 0;
}

static int centrino_cpu_exit(struct cpufreq_policy *policy)
{
	if (!centrino_model)
		return -ENODEV;

	cpufreq_frequency_table_put_attr(policy->cpu);

#ifdef CONFIG_X86_SPEEDSTEP_CENTRINO_ACPI
	if (!centrino_model->model_name) {
		acpi_processor_unregister_performance(&p, policy->cpu);
		kfree(centrino_model->op_points);
		kfree(centrino_model);
	}
#endif

	centrino_model = NULL;

	return 0;
}

/**
 * centrino_verify - verifies a new CPUFreq policy
 * @policy: new policy
 *
 * Limit must be within this model's frequency range at least one
 * border included.
 */
static int centrino_verify (struct cpufreq_policy *policy)
{
	return cpufreq_frequency_table_verify(policy, centrino_model->op_points);
}

/**
 * centrino_setpolicy - set a new CPUFreq policy
 * @policy: new policy
 * @target_freq: the target frequency
 * @relation: how that frequency relates to achieved frequency (CPUFREQ_RELATION_L or CPUFREQ_RELATION_H)
 *
 * Sets a new CPUFreq policy.
 */
static int centrino_target (struct cpufreq_policy *policy,
			    unsigned int target_freq,
			    unsigned int relation)
{
	unsigned int    newstate = 0;
	unsigned int	msr, oldmsr, h;
	struct cpufreq_freqs	freqs;
	cpumask_t		saved_mask;
	int			retval;

	if (centrino_model == NULL)
		return -ENODEV;

	/*
	 * Support for SMP systems.
	 * Make sure we are running on the CPU that wants to change frequency
	 */
	saved_mask = current->cpus_allowed;
	set_cpus_allowed(current, cpumask_of_cpu(policy->cpu));
	if (smp_processor_id() != policy->cpu) {
		return(-EAGAIN);
	}

	if (cpufreq_frequency_table_target(policy, centrino_model->op_points, target_freq,
					   relation, &newstate)) {
		retval = -EINVAL;
		goto migrate_end;
	}

	msr = centrino_model->op_points[newstate].index;
	rdmsr(MSR_IA32_PERF_CTL, oldmsr, h);

	if (msr == (oldmsr & 0xffff)) {
		retval = 0;
		goto migrate_end;
	}

	/* Hm, old frequency can either be the last value we put in
	   PERF_CTL, or whatever it is now. The trouble is that TM2
	   can change it behind our back, which means we never get to
	   see the speed change.  Reading back the current speed would
	   tell us something happened, but it may leave the things on
	   the notifier chain confused; we therefore stick to using
	   the last programmed speed rather than the current speed for
	   "old".

	   TODO: work out how the TCC interrupts work, and try to
	   catch the CPU changing things under us.
	*/
	freqs.cpu = policy->cpu;
	freqs.old = extract_clock(oldmsr);
	freqs.new = extract_clock(msr);

	dprintk(KERN_INFO PFX "target=%dkHz old=%d new=%d msr=%04x\n",
		target_freq, freqs.old, freqs.new, msr);

	cpufreq_notify_transition(&freqs, CPUFREQ_PRECHANGE);

	/* all but 16 LSB are "reserved", so treat them with
	   care */
	oldmsr &= ~0xffff;
	msr &= 0xffff;
	oldmsr |= msr;

	wrmsr(MSR_IA32_PERF_CTL, oldmsr, h);

	cpufreq_notify_transition(&freqs, CPUFREQ_POSTCHANGE);

	retval = 0;
migrate_end:
	set_cpus_allowed(current, saved_mask);
	return (retval);
}

static struct freq_attr* centrino_attr[] = {
	&cpufreq_freq_attr_scaling_available_freqs,
	NULL,
};

static struct cpufreq_driver centrino_driver = {
	.name		= "centrino", /* should be speedstep-centrino,
					 but there's a 16 char limit */
	.init		= centrino_cpu_init,
	.exit		= centrino_cpu_exit,
	.verify		= centrino_verify,
	.target		= centrino_target,
	.get		= get_cur_freq,
	.attr           = centrino_attr,
	.owner		= THIS_MODULE,
};


/**
 * centrino_init - initializes the Enhanced SpeedStep CPUFreq driver
 *
 * Initializes the Enhanced SpeedStep support. Returns -ENODEV on
 * unsupported devices, -ENOENT if there's no voltage table for this
 * particular CPU model, -EINVAL on problems during initiatization,
 * and zero on success.
 *
 * This is quite picky.  Not only does the CPU have to advertise the
 * "est" flag in the cpuid capability flags, we look for a specific
 * CPU model and stepping, and we need to have the exact model name in
 * our voltage tables.  That is, be paranoid about not releasing
 * someone's valuable magic smoke.
 */
static int __init centrino_init(void)
{
	struct cpuinfo_x86 *cpu = cpu_data;

	if (!cpu_has(cpu, X86_FEATURE_EST))
		return -ENODEV;

	return cpufreq_register_driver(&centrino_driver);
}

static void __exit centrino_exit(void)
{
	cpufreq_unregister_driver(&centrino_driver);
}

MODULE_AUTHOR ("Jeremy Fitzhardinge <[e-mail]>");
MODULE_DESCRIPTION ("Enhanced SpeedStep driver for Intel Pentium M processors.");
MODULE_LICENSE ("GPL");

late_initcall(centrino_init);
module_exit(centrino_exit);