1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
|
/*
* linux/arch/arm/mach-integrator/cpu.c
*
* Copyright (C) 2001 Deep Blue Solutions Ltd.
*
* $Id: cpu.c,v 1.2 2001/09/22 12:11:17 rmk Exp $
*
* 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.
*
* CPU support functions
*/
#include <linux/config.h>
#include <linux/types.h>
#include <linux/kernel.h>
#include <linux/cpufreq.h>
#include <linux/init.h>
#include <asm/hardware.h>
#include <asm/io.h>
#define CM_ID (IO_ADDRESS(INTEGRATOR_HDR_BASE)+INTEGRATOR_HDR_ID_OFFSET)
#define CM_OSC (IO_ADDRESS(INTEGRATOR_HDR_BASE)+INTEGRATOR_HDR_OSC_OFFSET)
#define CM_STAT (IO_ADDRESS(INTEGRATOR_HDR_BASE)+INTEGRATOR_HDR_STAT_OFFSET)
#define CM_LOCK (IO_ADDRESS(INTEGRATOR_HDR_BASE)+INTEGRATOR_HDR_LOCK_OFFSET)
struct vco {
unsigned char vdw;
unsigned char od;
};
/*
* Divisors for each OD setting.
*/
static unsigned char cc_divisor[8] = { 10, 2, 8, 4, 5, 7, 9, 6 };
static unsigned int vco_to_freq(struct vco vco, int factor)
{
return 2000 * (vco.vdw + 8) / cc_divisor[vco.od] / factor;
}
#ifdef CONFIG_CPU_FREQ
/*
* Divisor indexes for in ascending divisor order
*/
static unsigned char s2od[] = { 1, 3, 4, 7, 5, 2, 6, 0 };
static struct vco freq_to_vco(unsigned int freq_khz, int factor)
{
struct vco vco = {0, 0};
unsigned int i, f;
freq_khz *= factor;
for (i = 0; i < 8; i++) {
f = freq_khz * cc_divisor[s2od[i]];
/* f must be between 10MHz and 320MHz */
if (f > 10000 && f <= 320000)
break;
}
vco.od = s2od[i];
vco.vdw = f / 2000 - 8;
return vco;
}
/*
* Validate the speed in khz. If it is outside our
* range, then return the lowest.
*/
unsigned int integrator_validatespeed(unsigned int freq_khz)
{
struct vco vco;
if (freq_khz < 12000)
freq_khz = 12000;
if (freq_khz > 160000)
freq_khz = 160000;
vco = freq_to_vco(freq_khz, 1);
if (vco.vdw < 4 || vco.vdw > 152)
return -EINVAL;
return vco_to_freq(vco, 1);
}
void integrator_setspeed(unsigned int freq_khz)
{
struct vco vco = freq_to_vco(freq_khz, 1);
u_int cm_osc;
cm_osc = __raw_readl(CM_OSC);
cm_osc &= 0xfffff800;
cm_osc |= vco.vdw | vco.od << 8;
__raw_writel(0xa05f, CM_LOCK);
__raw_writel(cm_osc, CM_OSC);
__raw_writel(0, CM_LOCK);
}
#endif
static int __init cpu_init(void)
{
u_int cm_osc, cm_stat, cpu_freq_khz, mem_freq_khz;
struct vco vco;
cm_osc = __raw_readl(CM_OSC);
vco.od = (cm_osc >> 20) & 7;
vco.vdw = (cm_osc >> 12) & 255;
mem_freq_khz = vco_to_freq(vco, 2);
printk(KERN_INFO "Memory clock = %d.%03d MHz\n",
mem_freq_khz / 1000, mem_freq_khz % 1000);
vco.od = (cm_osc >> 8) & 7;
vco.vdw = cm_osc & 255;
cpu_freq_khz = vco_to_freq(vco, 1);
#ifdef CONFIG_CPU_FREQ
cpufreq_init(cpu_freq_khz);
cpufreq_setfunctions(integrator_validatespeed, integrator_setspeed);
#endif
cm_stat = __raw_readl(CM_STAT);
printk("Module id: %d\n", cm_stat & 255);
return 0;
}
__initcall(cpu_init);
|
