linux/drivers/clk/mmp/clk-mix.c
<<
>>
Prefs
   1/*
   2 * mmp mix(div and mux) clock operation source file
   3 *
   4 * Copyright (C) 2014 Marvell
   5 * Chao Xie <chao.xie@marvell.com>
   6 *
   7 * This file is licensed under the terms of the GNU General Public
   8 * License version 2. This program is licensed "as is" without any
   9 * warranty of any kind, whether express or implied.
  10 */
  11
  12#include <linux/clk-provider.h>
  13#include <linux/slab.h>
  14#include <linux/io.h>
  15#include <linux/err.h>
  16
  17#include "clk.h"
  18
  19/*
  20 * The mix clock is a clock combined mux and div type clock.
  21 * Because the div field and mux field need to be set at same
  22 * time, we can not divide it into 2 types of clock
  23 */
  24
  25#define to_clk_mix(hw)  container_of(hw, struct mmp_clk_mix, hw)
  26
  27static unsigned int _get_maxdiv(struct mmp_clk_mix *mix)
  28{
  29        unsigned int div_mask = (1 << mix->reg_info.width_div) - 1;
  30        unsigned int maxdiv = 0;
  31        struct clk_div_table *clkt;
  32
  33        if (mix->div_flags & CLK_DIVIDER_ONE_BASED)
  34                return div_mask;
  35        if (mix->div_flags & CLK_DIVIDER_POWER_OF_TWO)
  36                return 1 << div_mask;
  37        if (mix->div_table) {
  38                for (clkt = mix->div_table; clkt->div; clkt++)
  39                        if (clkt->div > maxdiv)
  40                                maxdiv = clkt->div;
  41                return maxdiv;
  42        }
  43        return div_mask + 1;
  44}
  45
  46static unsigned int _get_div(struct mmp_clk_mix *mix, unsigned int val)
  47{
  48        struct clk_div_table *clkt;
  49
  50        if (mix->div_flags & CLK_DIVIDER_ONE_BASED)
  51                return val;
  52        if (mix->div_flags & CLK_DIVIDER_POWER_OF_TWO)
  53                return 1 << val;
  54        if (mix->div_table) {
  55                for (clkt = mix->div_table; clkt->div; clkt++)
  56                        if (clkt->val == val)
  57                                return clkt->div;
  58                if (clkt->div == 0)
  59                        return 0;
  60        }
  61        return val + 1;
  62}
  63
  64static unsigned int _get_mux(struct mmp_clk_mix *mix, unsigned int val)
  65{
  66        int num_parents = clk_hw_get_num_parents(&mix->hw);
  67        int i;
  68
  69        if (mix->mux_flags & CLK_MUX_INDEX_BIT)
  70                return ffs(val) - 1;
  71        if (mix->mux_flags & CLK_MUX_INDEX_ONE)
  72                return val - 1;
  73        if (mix->mux_table) {
  74                for (i = 0; i < num_parents; i++)
  75                        if (mix->mux_table[i] == val)
  76                                return i;
  77                if (i == num_parents)
  78                        return 0;
  79        }
  80
  81        return val;
  82}
  83static unsigned int _get_div_val(struct mmp_clk_mix *mix, unsigned int div)
  84{
  85        struct clk_div_table *clkt;
  86
  87        if (mix->div_flags & CLK_DIVIDER_ONE_BASED)
  88                return div;
  89        if (mix->div_flags & CLK_DIVIDER_POWER_OF_TWO)
  90                return __ffs(div);
  91        if (mix->div_table) {
  92                for (clkt = mix->div_table; clkt->div; clkt++)
  93                        if (clkt->div == div)
  94                                return clkt->val;
  95                if (clkt->div == 0)
  96                        return 0;
  97        }
  98
  99        return div - 1;
 100}
 101
 102static unsigned int _get_mux_val(struct mmp_clk_mix *mix, unsigned int mux)
 103{
 104        if (mix->mux_table)
 105                return mix->mux_table[mux];
 106
 107        return mux;
 108}
 109
 110static void _filter_clk_table(struct mmp_clk_mix *mix,
 111                                struct mmp_clk_mix_clk_table *table,
 112                                unsigned int table_size)
 113{
 114        int i;
 115        struct mmp_clk_mix_clk_table *item;
 116        struct clk_hw *parent, *hw;
 117        unsigned long parent_rate;
 118
 119        hw = &mix->hw;
 120
 121        for (i = 0; i < table_size; i++) {
 122                item = &table[i];
 123                parent = clk_hw_get_parent_by_index(hw, item->parent_index);
 124                parent_rate = clk_hw_get_rate(parent);
 125                if (parent_rate % item->rate) {
 126                        item->valid = 0;
 127                } else {
 128                        item->divisor = parent_rate / item->rate;
 129                        item->valid = 1;
 130                }
 131        }
 132}
 133
 134static int _set_rate(struct mmp_clk_mix *mix, u32 mux_val, u32 div_val,
 135                        unsigned int change_mux, unsigned int change_div)
 136{
 137        struct mmp_clk_mix_reg_info *ri = &mix->reg_info;
 138        u8 width, shift;
 139        u32 mux_div, fc_req;
 140        int ret, timeout = 50;
 141        unsigned long flags = 0;
 142
 143        if (!change_mux && !change_div)
 144                return -EINVAL;
 145
 146        if (mix->lock)
 147                spin_lock_irqsave(mix->lock, flags);
 148
 149        if (mix->type == MMP_CLK_MIX_TYPE_V1
 150                || mix->type == MMP_CLK_MIX_TYPE_V2)
 151                mux_div = readl(ri->reg_clk_ctrl);
 152        else
 153                mux_div = readl(ri->reg_clk_sel);
 154
 155        if (change_div) {
 156                width = ri->width_div;
 157                shift = ri->shift_div;
 158                mux_div &= ~MMP_CLK_BITS_MASK(width, shift);
 159                mux_div |= MMP_CLK_BITS_SET_VAL(div_val, width, shift);
 160        }
 161
 162        if (change_mux) {
 163                width = ri->width_mux;
 164                shift = ri->shift_mux;
 165                mux_div &= ~MMP_CLK_BITS_MASK(width, shift);
 166                mux_div |= MMP_CLK_BITS_SET_VAL(mux_val, width, shift);
 167        }
 168
 169        if (mix->type == MMP_CLK_MIX_TYPE_V1) {
 170                writel(mux_div, ri->reg_clk_ctrl);
 171        } else if (mix->type == MMP_CLK_MIX_TYPE_V2) {
 172                mux_div |= (1 << ri->bit_fc);
 173                writel(mux_div, ri->reg_clk_ctrl);
 174
 175                do {
 176                        fc_req = readl(ri->reg_clk_ctrl);
 177                        timeout--;
 178                        if (!(fc_req & (1 << ri->bit_fc)))
 179                                break;
 180                } while (timeout);
 181
 182                if (timeout == 0) {
 183                        pr_err("%s:%s cannot do frequency change\n",
 184                                __func__, clk_hw_get_name(&mix->hw));
 185                        ret = -EBUSY;
 186                        goto error;
 187                }
 188        } else {
 189                fc_req = readl(ri->reg_clk_ctrl);
 190                fc_req |= 1 << ri->bit_fc;
 191                writel(fc_req, ri->reg_clk_ctrl);
 192                writel(mux_div, ri->reg_clk_sel);
 193                fc_req &= ~(1 << ri->bit_fc);
 194        }
 195
 196        ret = 0;
 197error:
 198        if (mix->lock)
 199                spin_unlock_irqrestore(mix->lock, flags);
 200
 201        return ret;
 202}
 203
 204static int mmp_clk_mix_determine_rate(struct clk_hw *hw,
 205                                      struct clk_rate_request *req)
 206{
 207        struct mmp_clk_mix *mix = to_clk_mix(hw);
 208        struct mmp_clk_mix_clk_table *item;
 209        struct clk_hw *parent, *parent_best;
 210        unsigned long parent_rate, mix_rate, mix_rate_best, parent_rate_best;
 211        unsigned long gap, gap_best;
 212        u32 div_val_max;
 213        unsigned int div;
 214        int i, j;
 215
 216
 217        mix_rate_best = 0;
 218        parent_rate_best = 0;
 219        gap_best = ULONG_MAX;
 220        parent_best = NULL;
 221
 222        if (mix->table) {
 223                for (i = 0; i < mix->table_size; i++) {
 224                        item = &mix->table[i];
 225                        if (item->valid == 0)
 226                                continue;
 227                        parent = clk_hw_get_parent_by_index(hw,
 228                                                        item->parent_index);
 229                        parent_rate = clk_hw_get_rate(parent);
 230                        mix_rate = parent_rate / item->divisor;
 231                        gap = abs(mix_rate - req->rate);
 232                        if (!parent_best || gap < gap_best) {
 233                                parent_best = parent;
 234                                parent_rate_best = parent_rate;
 235                                mix_rate_best = mix_rate;
 236                                gap_best = gap;
 237                                if (gap_best == 0)
 238                                        goto found;
 239                        }
 240                }
 241        } else {
 242                for (i = 0; i < clk_hw_get_num_parents(hw); i++) {
 243                        parent = clk_hw_get_parent_by_index(hw, i);
 244                        parent_rate = clk_hw_get_rate(parent);
 245                        div_val_max = _get_maxdiv(mix);
 246                        for (j = 0; j < div_val_max; j++) {
 247                                div = _get_div(mix, j);
 248                                mix_rate = parent_rate / div;
 249                                gap = abs(mix_rate - req->rate);
 250                                if (!parent_best || gap < gap_best) {
 251                                        parent_best = parent;
 252                                        parent_rate_best = parent_rate;
 253                                        mix_rate_best = mix_rate;
 254                                        gap_best = gap;
 255                                        if (gap_best == 0)
 256                                                goto found;
 257                                }
 258                        }
 259                }
 260        }
 261
 262found:
 263        if (!parent_best)
 264                return -EINVAL;
 265
 266        req->best_parent_rate = parent_rate_best;
 267        req->best_parent_hw = parent_best;
 268        req->rate = mix_rate_best;
 269
 270        return 0;
 271}
 272
 273static int mmp_clk_mix_set_rate_and_parent(struct clk_hw *hw,
 274                                                unsigned long rate,
 275                                                unsigned long parent_rate,
 276                                                u8 index)
 277{
 278        struct mmp_clk_mix *mix = to_clk_mix(hw);
 279        unsigned int div;
 280        u32 div_val, mux_val;
 281
 282        div = parent_rate / rate;
 283        div_val = _get_div_val(mix, div);
 284        mux_val = _get_mux_val(mix, index);
 285
 286        return _set_rate(mix, mux_val, div_val, 1, 1);
 287}
 288
 289static u8 mmp_clk_mix_get_parent(struct clk_hw *hw)
 290{
 291        struct mmp_clk_mix *mix = to_clk_mix(hw);
 292        struct mmp_clk_mix_reg_info *ri = &mix->reg_info;
 293        unsigned long flags = 0;
 294        u32 mux_div = 0;
 295        u8 width, shift;
 296        u32 mux_val;
 297
 298        if (mix->lock)
 299                spin_lock_irqsave(mix->lock, flags);
 300
 301        if (mix->type == MMP_CLK_MIX_TYPE_V1
 302                || mix->type == MMP_CLK_MIX_TYPE_V2)
 303                mux_div = readl(ri->reg_clk_ctrl);
 304        else
 305                mux_div = readl(ri->reg_clk_sel);
 306
 307        if (mix->lock)
 308                spin_unlock_irqrestore(mix->lock, flags);
 309
 310        width = mix->reg_info.width_mux;
 311        shift = mix->reg_info.shift_mux;
 312
 313        mux_val = MMP_CLK_BITS_GET_VAL(mux_div, width, shift);
 314
 315        return _get_mux(mix, mux_val);
 316}
 317
 318static unsigned long mmp_clk_mix_recalc_rate(struct clk_hw *hw,
 319                                        unsigned long parent_rate)
 320{
 321        struct mmp_clk_mix *mix = to_clk_mix(hw);
 322        struct mmp_clk_mix_reg_info *ri = &mix->reg_info;
 323        unsigned long flags = 0;
 324        u32 mux_div = 0;
 325        u8 width, shift;
 326        unsigned int div;
 327
 328        if (mix->lock)
 329                spin_lock_irqsave(mix->lock, flags);
 330
 331        if (mix->type == MMP_CLK_MIX_TYPE_V1
 332                || mix->type == MMP_CLK_MIX_TYPE_V2)
 333                mux_div = readl(ri->reg_clk_ctrl);
 334        else
 335                mux_div = readl(ri->reg_clk_sel);
 336
 337        if (mix->lock)
 338                spin_unlock_irqrestore(mix->lock, flags);
 339
 340        width = mix->reg_info.width_div;
 341        shift = mix->reg_info.shift_div;
 342
 343        div = _get_div(mix, MMP_CLK_BITS_GET_VAL(mux_div, width, shift));
 344
 345        return parent_rate / div;
 346}
 347
 348static int mmp_clk_set_parent(struct clk_hw *hw, u8 index)
 349{
 350        struct mmp_clk_mix *mix = to_clk_mix(hw);
 351        struct mmp_clk_mix_clk_table *item;
 352        int i;
 353        u32 div_val, mux_val;
 354
 355        if (mix->table) {
 356                for (i = 0; i < mix->table_size; i++) {
 357                        item = &mix->table[i];
 358                        if (item->valid == 0)
 359                                continue;
 360                        if (item->parent_index == index)
 361                                break;
 362                }
 363                if (i < mix->table_size) {
 364                        div_val = _get_div_val(mix, item->divisor);
 365                        mux_val = _get_mux_val(mix, item->parent_index);
 366                } else
 367                        return -EINVAL;
 368        } else {
 369                mux_val = _get_mux_val(mix, index);
 370                div_val = 0;
 371        }
 372
 373        return _set_rate(mix, mux_val, div_val, 1, div_val ? 1 : 0);
 374}
 375
 376static int mmp_clk_set_rate(struct clk_hw *hw, unsigned long rate,
 377                                unsigned long best_parent_rate)
 378{
 379        struct mmp_clk_mix *mix = to_clk_mix(hw);
 380        struct mmp_clk_mix_clk_table *item;
 381        unsigned long parent_rate;
 382        unsigned int best_divisor;
 383        struct clk_hw *parent;
 384        int i;
 385
 386        best_divisor = best_parent_rate / rate;
 387
 388        if (mix->table) {
 389                for (i = 0; i < mix->table_size; i++) {
 390                        item = &mix->table[i];
 391                        if (item->valid == 0)
 392                                continue;
 393                        parent = clk_hw_get_parent_by_index(hw,
 394                                                        item->parent_index);
 395                        parent_rate = clk_hw_get_rate(parent);
 396                        if (parent_rate == best_parent_rate
 397                                && item->divisor == best_divisor)
 398                                break;
 399                }
 400                if (i < mix->table_size)
 401                        return _set_rate(mix,
 402                                        _get_mux_val(mix, item->parent_index),
 403                                        _get_div_val(mix, item->divisor),
 404                                        1, 1);
 405                else
 406                        return -EINVAL;
 407        } else {
 408                for (i = 0; i < clk_hw_get_num_parents(hw); i++) {
 409                        parent = clk_hw_get_parent_by_index(hw, i);
 410                        parent_rate = clk_hw_get_rate(parent);
 411                        if (parent_rate == best_parent_rate)
 412                                break;
 413                }
 414                if (i < clk_hw_get_num_parents(hw))
 415                        return _set_rate(mix, _get_mux_val(mix, i),
 416                                        _get_div_val(mix, best_divisor), 1, 1);
 417                else
 418                        return -EINVAL;
 419        }
 420}
 421
 422static void mmp_clk_mix_init(struct clk_hw *hw)
 423{
 424        struct mmp_clk_mix *mix = to_clk_mix(hw);
 425
 426        if (mix->table)
 427                _filter_clk_table(mix, mix->table, mix->table_size);
 428}
 429
 430const struct clk_ops mmp_clk_mix_ops = {
 431        .determine_rate = mmp_clk_mix_determine_rate,
 432        .set_rate_and_parent = mmp_clk_mix_set_rate_and_parent,
 433        .set_rate = mmp_clk_set_rate,
 434        .set_parent = mmp_clk_set_parent,
 435        .get_parent = mmp_clk_mix_get_parent,
 436        .recalc_rate = mmp_clk_mix_recalc_rate,
 437        .init = mmp_clk_mix_init,
 438};
 439
 440struct clk *mmp_clk_register_mix(struct device *dev,
 441                                        const char *name,
 442                                        const char **parent_names,
 443                                        u8 num_parents,
 444                                        unsigned long flags,
 445                                        struct mmp_clk_mix_config *config,
 446                                        spinlock_t *lock)
 447{
 448        struct mmp_clk_mix *mix;
 449        struct clk *clk;
 450        struct clk_init_data init;
 451        size_t table_bytes;
 452
 453        mix = kzalloc(sizeof(*mix), GFP_KERNEL);
 454        if (!mix)
 455                return ERR_PTR(-ENOMEM);
 456
 457        init.name = name;
 458        init.flags = flags | CLK_GET_RATE_NOCACHE;
 459        init.parent_names = parent_names;
 460        init.num_parents = num_parents;
 461        init.ops = &mmp_clk_mix_ops;
 462
 463        memcpy(&mix->reg_info, &config->reg_info, sizeof(config->reg_info));
 464        if (config->table) {
 465                table_bytes = sizeof(*config->table) * config->table_size;
 466                mix->table = kmemdup(config->table, table_bytes, GFP_KERNEL);
 467                if (!mix->table)
 468                        goto free_mix;
 469
 470                mix->table_size = config->table_size;
 471        }
 472
 473        if (config->mux_table) {
 474                table_bytes = sizeof(u32) * num_parents;
 475                mix->mux_table = kmemdup(config->mux_table, table_bytes,
 476                                         GFP_KERNEL);
 477                if (!mix->mux_table) {
 478                        kfree(mix->table);
 479                        goto free_mix;
 480                }
 481        }
 482
 483        mix->div_flags = config->div_flags;
 484        mix->mux_flags = config->mux_flags;
 485        mix->lock = lock;
 486        mix->hw.init = &init;
 487
 488        if (config->reg_info.bit_fc >= 32)
 489                mix->type = MMP_CLK_MIX_TYPE_V1;
 490        else if (config->reg_info.reg_clk_sel)
 491                mix->type = MMP_CLK_MIX_TYPE_V3;
 492        else
 493                mix->type = MMP_CLK_MIX_TYPE_V2;
 494        clk = clk_register(dev, &mix->hw);
 495
 496        if (IS_ERR(clk)) {
 497                kfree(mix->mux_table);
 498                kfree(mix->table);
 499                kfree(mix);
 500        }
 501
 502        return clk;
 503
 504free_mix:
 505        kfree(mix);
 506        return ERR_PTR(-ENOMEM);
 507}
 508