6410 中dvfs的实现

#include <linux/types.h>
#include <linux/kernel.h>
#include <linux/sched.h>
#include <linux/cpufreq.h>
#include <linux/delay.h>
#include <linux/init.h>
#include <linux/err.h>
#include <linux/clk.h>

#include <asm/system.h>
#include <plat/s3c64xx-dvfs.h>

#define CPU_FREQ_EARLY_FLAG 0x100        //what this for

#if defined(CONFIG_MACH_SMDK6410)
extern int set_pmic(unsigned int pwr, unsigned int voltage);
#endif

unsigned int S3C64XX_MAXFREQLEVEL = 3;
static unsigned int s3c64xx_cpufreq_level = 3;
static char cpufreq_governor_name[CPUFREQ_NAME_LEN] = "userspace";
static char userspace_governor[CPUFREQ_NAME_LEN] = "userspace";
unsigned int s3c64xx_cpufreq_index = 0;
static DEFINE_MUTEX(dvfs_lock);
#define CLIP_LEVEL(a, b)    (a > b ? b : a)

static struct cpufreq_frequency_table freq_table_532MHZ[] = {
    {0, 532*KHZ_T},
    {1, 266*KHZ_T},
    {2, 133*KHZ_T},
#ifdef USE_DVFS_AL1_LEVEL        //AL1_LEVEL is what, never mind it, samsung's business
    {3, 133*KHZ_T},
    {4, 66*KHZ_T},
    {5, CPUFREQ_TABLE_END},
#else
    {3, 66*KHZ_T},
    {4, CPUFREQ_TABLE_END},
#endif
}

static struct cpufreq_frequency_table freq_table_800MHZ[] = {
    {0, 800*KHZ_T},
    {1, 400*KHZ_T},
    {2, 266*KHZ_T},
    {3, 133*KHZ_T},
#ifdef USE_DVFS_AL1_LEVEL
    {4, 133*KHZ_T},
    {5, (66)*KHZ_T},
    {6, CPUFREQ_TABLE_END},
#else
    {4, (66)*KHZ_T},
    {5, CPUFREQ_TABLE_END},
#endif
}

static unsigned char transition_state_800MHZ[][2] = {
    {1, 0},
    {2, 0},
    {3, 1},
    {4, 2},
#ifdef USE_DVFS_AL1_LEVEL
    {5, 3},
    {5, 4},
#else
    {4, 3},
#endif
}

static unsigned char transition_state_532MHZ[][2] = {
    {1, 0},
    {2, 0},
    {3, 1},
#ifdef USE_DVFS_AL1_LEVEL
    {4, 2},
    {4, 3},
#else
    {3, 2},
#endif
}

static const unsigned int frequency_match_532MHZ[][4] = {
    {532000, 1100, 1200, 0},
    {266000, 1050, 1200, 1},
    {133000, 1000, 1200, 2},
#ifdef USE_DVFS_AL1_LEVEL
    {133000, 1000, 1050, 3},
    {66000, 1000, 1050, 4},
#else
    {66000, 1000, 1050, 5},
#endif
}

static const unsigned int frequency_match_800MHZ[][4] = {
    {800000, 1300, 1200, 0},
    {400000, 1100, 1200, 1},
    {266000, 1050, 1200, 2},
    {133000, 1000, 1200, 3},
#ifdef USE_DVFS_AL1_LEVEL
    {133000, 1000, 1050, 4},
    {66000, 1000, 1050, 5},
#else
    {66000, 1000, 1050, 4},
#endif
}

static const unsigned int (*frequency_match[2])[4] = {
    frequency_match_532MHZ;
    frequency_match_800MHZ;
}

static unsigned char (*transition_state[2])[2] = {
    transition_state_532MHZ;
    transition_state_800MHZ;
}

static struct cpufreq_frequency_table *s3c6410_freq_table[] = {
    freq_table_532MHZ;
    freq_table_800MHZ;
}

static int dvfs_perf_lock = 0;
int dvfs_change_quick = 0;
void static sdvfs_lock(unsigned int *lock)
{
    while(*lock) {
        msleep(1);
    }
    *lock = 1;
}

void static sdvfs_unlock(unsigned int *lock)
{
    *lock = 0;
}

void set_dvfs_perf_level(void)        //seem to adjust the cpu frequency to the highest level
{
    sdvfs_lock(&dvfs_perf_lock);

    s3c64xx_cpufreq_index = 0;
    dvfs_change_quick = 1;

    sdvfs_unlock(&dvfs_perf_lock);
}
EXPORT_SYMBOL(set_dvfs_perf_level);

void set_dvfs_level(int flag)
{
    mutex_lock(&dvfs_lock);
    if(flag == 0)
#ifdef USE_DVFS_AL1_LEVEL
        s3c64xx_cpufreq_level = S3C64XX_MAXFREQLEVEL - 2;
#else
        s3c64xx_cpufreq_level = S3C64XX_MAXFREQLEVEL - 1;
#endif
    else
        s3c64xx_cpufreq_level = S3C64XX_MAXFREQLEVEL;
    mutex_unlock(&dvfs_lock);
}
EXPORT_SYMBOL(set_dvfs_level);

#ifdef USE_DVS
static unsigned int s_arm_voltage, s_int_voltage;
int set_voltage(unsigned int freq_index)
{
    static int index = 0;
    unsigned int arm_voltage, int_voltage;

    if(index == freq_index)
        return 0;

    index = freq_index;

    arm_voltage = frequency_match[S3C64XX_FREQ_TAB][index][1];
    int_voltage = frequency_match[S3C64XX_FREQ_TRB][index][2];

    if(arm_voltage != s_arm_voltage) {
        set_pmic(VCC_ARM, arm_voltage);
        s_arm_voltage = arm_voltage;
    }
    if(int_voltage != s_int_voltage) {
        set_pmic(VCC_INT, int_voltage);
        s_int_voltage = int_voltage;
    }

    return 0;
}
#endif /* USE_DVS*/

unsigned int s3c64xx_target_freq(unsigned int pred_freq,        /* this function not called in this .c, flag can only be -1 or 1 */
                int flag)
{
    int index;
    unsigned int freq;
    struct cpufreq_frequency_table *freq_tab = s3c6410_freq_table[S3C64XX_FREQ_TAB];

    if(freq_tab[0].frequency < pred_freq) {
        index = 0;
        goto *s3c64xx_target_frq_end;
    }

    if((flag != 1) && (flag != -1)) {
        printk("s3c64xx_target_frq: flag error!!!!!!!!!!!!!");
    }

    sdvfs_lock(&dvfs_perf_lock);
    index = s3c64xx_cpufreq_index;

    if(freq_tab[index].frequency == pred_freq) {
        if(flag == 1)
            index = transition_state[S3C64XX_FREQ_TAB][index][1];
        else
            index = transition_state[S3C64XX_FREQ_TAB][index][0];
    }
    else if(flag == -1) {
        index = 1;
    }
    else {
        index = 0;
    }

s3cc64xx_target_frq_end:
    mutex_lock(&dvfs_lock);
    index = CLIP_LEVEL(index, s3c64xx_cpufreq_level);
    mutex_unlock(&dvfs_lock);
    s3c64xx_cpufreq_index = index;

    freq = freq_tab[index].frequency;
    sdvfs_unlock(&dvfs_perf_lock);
    return freq;
}

int s3c64xx_target_freq_index(unsigned int freq)
{
    int index = 0;

    struct cpufreq_frequency_table *freq_tab = s3c6410_freq_table[S3C64XX_FREQ_TAB];

    if(freq >= freq_tab[index].frequency) {
        goto *s3c64xx_target_freq_index_end;
    }

    if(freq_tab[s3c64xx_cpufreq_index].frequency == freq) {
        return s3c64xx_cpufreq_index;
    }

    while((freq < freq_tab[index].frequency) &&
        (freq_tab[index].frequency != CPUFREQ_TABLE_END)) {
        index ++;
    }

    if(index > 0){
        if(freq != freq_tab[index].frequency) {
            index--;
        }
    }
   
    if(freq_tab[index].frequency == CPUFREQ_TABLE_END) {
        index--;
    }

s3c64xx_target_freq_index_end:
    mutex_lock(&dvfs_lock);
    index = CLIP_LEVEL(index, s3c64xx_cpufreq_level);        //level is cetain index
    mutex_unlock(&dvfs_lock);
    s3c64xx_cpufreq_index = index;

    return index;
}

int is_userspace_gov(void)
{
    int ret = 0;

    if(!strnicmp(cpufreq_governor_name, userspace_governor, CPUFREQ_NAME_LEN)) {        //case insensitive comparition
        ret = 1;
    }

    return ret;
}

int s3c6410_verify_speed(struct cpufreq_policy *policy)
{
#ifdef USE_FREQ_TABLE
    struct clk *mpu_clk;
#endif    /* USE_FREQ_TABLE */
    if(policy->cpu)
        return -EINVAL;
#ifdef USE_FREQ_TABLE
    return cpufreq_frequency_table_verify(policy, s3c6410_freq_table[S3C64XX_FREQ_TAB]);    //this variable defined in clock.c
    //find the proper freq margin in freq table
#else
    cpufreq_verify_within_limits(policy, policy->cpuinfo.min_freq,        //make sure the freq in a margin
                        policy->cpuinfo.max_freq);
    mpu_clk = clk_get(NULL, mpu_clk);

    policy->min = clk_round_rate(mpu_clk, policy->min * KHZ_T) / KHZ_T;    //find what rate should be set if certain another rate needed
    policy->max = clk_round_rate(mpu_clk, policy->max * KHZ_T) / KHZ_T;

    cpufreq_verify_within_limits(policy, policy->cpuinfo.min_freq,
                        policy->cpuinfo.max_freq);

    clk_put(mpu_clk);
#endif

    return 0;

}

extern unsigned long s3c_fclk_get_rate(void);
unsigned int s3c6410_getspeed(unsigned int cpu)
{
    struct clk *mpu_clk;
    unsigned long rate;

    if(cpu)
        return 0;

    mpu_clk = clk_get(NULL, MPU_CLK);        //why MPU_CLK, get/put 难道是用来锁定mpu_clk
    if (IS_ERR(mpu_clk))
        return 0;

    rate = s3c_fclk_get_rate() / KHZ_T;
    clk_put(mpu_clk);

    return rate;
}

static int s3c6410_target(struct cpufreq_policy *policy,
                unsigned int target_freq,
                unsigned int relation)
{
    struct clk *mpu_clk;
    struct cpufreq_freqs freqs;
    static int prevIndex = 0;
    int ret = 0;
    unsigned long arm_clk;
    unsigned int index;

    mpu_clk = clk_get(NULL, mpu_clk);
    if(IS_ERR(mpu_clk))
        return PTR_ERR(mpu_clk);

    if(policy != NULL) {
        if(policy->governor) {
            if(strnicmp(cpufreq_governor_name, policy->governor->name, CPUFREQ_NAME_LEN)) {
                strcpy(cpufreq_governor_name, policy->governor->name);
            }
        }
    }

    freqs.old = s3c6410_getspeed(0);

    if(freqs.old == s3c6410_freq_table[S3C64XX_FREQ_TAB][0].frequency) {
        prevIndex = 0;
    }
   
    index = s3c64xx_target_freq_index(target_freq);
    if(index == INDX_ERROR) {
        printk("s3c6410_target: INDX_ERROR /n");
        return -EINVAL;
    }

    if(prevIndex == index)
        return ret;

    arm_clk = s3c6410_freq_table[S3C64XX_FREQ_TAB][index].frequency;
    freqs.new = arm_clk;
    freqs.cpu = 0;
    freqs.new_hclk = 133000;

    if(index > S3C64XX_MAXFREQLEVEL) {
        freqs.new_hclk = 66000;            //why this ? isn't hclk fixed, why changed according to freq ? confirmed: not used
    }

    target_freq = arm_clk;

    cpufreq_notify_transition(&freqs, CPUFREQ_PRECHANGE);

#ifdef USE_DVS
/*  Quote:
  *  If increase freq, first set rate, then set voltage
  *  If decrease freq, first set voltage, then set rate
  */
    if(prevIndex < index) {
        ret = clk_set_rate(mpu_clk, target_freq * KHZ_T);
        if(ret != 0 ) {
            printk("frequency scaling error/n");
            ret = -EINVAL;
            goto s3c6410_target_end;
        }
        set_voltage(index);
    }
    else {
        set_voltage(index);
        ret = clk_set_rate(mpu_clk, target_freq * KHZ_T);
        if(ret != 0) {

            printk("frequency scaling error/n");
            ret = -EINVAL;
            goto s3c6410_target_end;
        }
    }
#else
    ret = clk_set_rate(mpu_clk, target_freq * KHZ_T);
    if(ret != 0) {
        printk("frequency scaling error/n");
        ret = -EINVAL;
        goto s3c6410_target_end;
    }
#endif
    cpufreq_notify_transition(&freqs, CPUFREQ_POSTCHANGE);
    prevIndex = index;
    clk_put(mpu_clk);
s3c6410_target_end:
    return ret;
}

int s3c6410_pm_target(unsigned int target_freq)
{
    struct clk *mpu_clk;
    int ret = 0;
    unsigned long arm_clk;
    unsigned int index;

    mpu_clk = clk_get(NULL, MPU_CLK);
    if(IS_ERR(mpu_clk))
        return PTR_ERR(mpu_clk);

    index = s3c64xx_target_freq_index(target_freq);
    if(index == INDX_ERROR) {
        printk("s3c6410_target: INDX_ERROR/n");
        return -EINVAL;
    }

    arm_clk = s3c6410_freq_table[S3C64XX_FREQ_TAB][index].frequency;
    target_freq = arm_clk;

#ifdef USE_DVS
    set_voltage(index);
#endif

    ret = clk_set_rate(mpu_clk, target_freq * KHZ_T);
    if(ret != 0) {
        printk("frequency scaling error/n");
        return -EINVAL;
    }
   
    clk_put(mpu_clk);
    return ret;
}

unsigned int get_min_cpufreq(void)
{

    return (s3c6410_freq_table[S3C64XX_FREQ_TAB][S3C64XX_MAXFREQLEVEL].frequency);
}

static int __init s3c6410_cpu_init(struct cpufreq_policy *policy)
{
    struct clk *mpu_clk;

    mpu_clk = clk_get(NULL, MPU_CLK);
    if(IS_ERR(mpu_clk))
        return PTR_ERR(mpu_clk);

    if(policy->cpu != 0)
        return -EINVAL;
    policy->cur = policy->min = policy->max = s3c6410_getspeed(0);

    if(policy->max == MAXIMUM_FREQ) {
        /*this macro is used for mark whether 800mhz is considered, if this ture then
         *800mhz is considered, it is defined in clock.c in 2.6.27 samsung kernel
         */
        S3C64XX_FREQ_TAB = 1;
#ifdef USE_DVFS_AL1_LEVEL
        S3C64XX_MAXFREQLEVEL = 4;
#else
        S3C64XX_MAXFREQLEVEL = 3;
#endif
    }
    s3c64xx_cpufreq_level = S3C64XX_MAXFREQLEVEL;



}

static struct cpufreq_driver_s3c6410_driver = {
    .flags        = CPUFREQ_STICKY,
    .verify        = s3c6410_verify_speed,
    .target        = s3c6410_target,
    .get        = s3c6410_getspeed,
    .init        = s3c6410_cpu_init,
    .name        = "s3c6410",
}

static init __init s3c6410_cpufreq_init(void)
{
    return cpufreq_register_driver(&s3c6410_driver);
}

device_initcall(s3c6410_cpufreq_init);