linux/drivers/pinctrl/pinctrl-equilibrium.c
<<
>>
Prefs
   1// SPDX-License-Identifier: GPL-2.0
   2/* Copyright (C) 2019 Intel Corporation */
   3
   4#include <linux/gpio/driver.h>
   5#include <linux/module.h>
   6#include <linux/of.h>
   7#include <linux/of_address.h>
   8#include <linux/of_irq.h>
   9#include <linux/pinctrl/pinctrl.h>
  10#include <linux/pinctrl/pinconf.h>
  11#include <linux/pinctrl/pinconf-generic.h>
  12#include <linux/pinctrl/pinmux.h>
  13#include <linux/platform_device.h>
  14
  15#include "core.h"
  16#include "pinconf.h"
  17#include "pinmux.h"
  18#include "pinctrl-equilibrium.h"
  19
  20#define PIN_NAME_FMT    "io-%d"
  21#define PIN_NAME_LEN    10
  22#define PAD_REG_OFF     0x100
  23
  24static void eqbr_gpio_disable_irq(struct irq_data *d)
  25{
  26        struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
  27        struct eqbr_gpio_ctrl *gctrl = gpiochip_get_data(gc);
  28        unsigned int offset = irqd_to_hwirq(d);
  29        unsigned long flags;
  30
  31        raw_spin_lock_irqsave(&gctrl->lock, flags);
  32        writel(BIT(offset), gctrl->membase + GPIO_IRNENCLR);
  33        raw_spin_unlock_irqrestore(&gctrl->lock, flags);
  34}
  35
  36static void eqbr_gpio_enable_irq(struct irq_data *d)
  37{
  38        struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
  39        struct eqbr_gpio_ctrl *gctrl = gpiochip_get_data(gc);
  40        unsigned int offset = irqd_to_hwirq(d);
  41        unsigned long flags;
  42
  43        gc->direction_input(gc, offset);
  44        raw_spin_lock_irqsave(&gctrl->lock, flags);
  45        writel(BIT(offset), gctrl->membase + GPIO_IRNRNSET);
  46        raw_spin_unlock_irqrestore(&gctrl->lock, flags);
  47}
  48
  49static void eqbr_gpio_ack_irq(struct irq_data *d)
  50{
  51        struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
  52        struct eqbr_gpio_ctrl *gctrl = gpiochip_get_data(gc);
  53        unsigned int offset = irqd_to_hwirq(d);
  54        unsigned long flags;
  55
  56        raw_spin_lock_irqsave(&gctrl->lock, flags);
  57        writel(BIT(offset), gctrl->membase + GPIO_IRNCR);
  58        raw_spin_unlock_irqrestore(&gctrl->lock, flags);
  59}
  60
  61static void eqbr_gpio_mask_ack_irq(struct irq_data *d)
  62{
  63        eqbr_gpio_disable_irq(d);
  64        eqbr_gpio_ack_irq(d);
  65}
  66
  67static inline void eqbr_cfg_bit(void __iomem *addr,
  68                                unsigned int offset, unsigned int set)
  69{
  70        if (set)
  71                writel(readl(addr) | BIT(offset), addr);
  72        else
  73                writel(readl(addr) & ~BIT(offset), addr);
  74}
  75
  76static int eqbr_irq_type_cfg(struct gpio_irq_type *type,
  77                             struct eqbr_gpio_ctrl *gctrl,
  78                             unsigned int offset)
  79{
  80        unsigned long flags;
  81
  82        raw_spin_lock_irqsave(&gctrl->lock, flags);
  83        eqbr_cfg_bit(gctrl->membase + GPIO_IRNCFG, offset, type->trig_type);
  84        eqbr_cfg_bit(gctrl->membase + GPIO_EXINTCR1, offset, type->trig_type);
  85        eqbr_cfg_bit(gctrl->membase + GPIO_EXINTCR0, offset, type->logic_type);
  86        raw_spin_unlock_irqrestore(&gctrl->lock, flags);
  87
  88        return 0;
  89}
  90
  91static int eqbr_gpio_set_irq_type(struct irq_data *d, unsigned int type)
  92{
  93        struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
  94        struct eqbr_gpio_ctrl *gctrl = gpiochip_get_data(gc);
  95        unsigned int offset = irqd_to_hwirq(d);
  96        struct gpio_irq_type it;
  97
  98        memset(&it, 0, sizeof(it));
  99
 100        if ((type & IRQ_TYPE_SENSE_MASK) == IRQ_TYPE_NONE)
 101                return 0;
 102
 103        switch (type) {
 104        case IRQ_TYPE_EDGE_RISING:
 105                it.trig_type = GPIO_EDGE_TRIG;
 106                it.edge_type = GPIO_SINGLE_EDGE;
 107                it.logic_type = GPIO_POSITIVE_TRIG;
 108                break;
 109
 110        case IRQ_TYPE_EDGE_FALLING:
 111                it.trig_type = GPIO_EDGE_TRIG;
 112                it.edge_type = GPIO_SINGLE_EDGE;
 113                it.logic_type = GPIO_NEGATIVE_TRIG;
 114                break;
 115
 116        case IRQ_TYPE_EDGE_BOTH:
 117                it.trig_type = GPIO_EDGE_TRIG;
 118                it.edge_type = GPIO_BOTH_EDGE;
 119                it.logic_type = GPIO_POSITIVE_TRIG;
 120                break;
 121
 122        case IRQ_TYPE_LEVEL_HIGH:
 123                it.trig_type = GPIO_LEVEL_TRIG;
 124                it.edge_type = GPIO_SINGLE_EDGE;
 125                it.logic_type = GPIO_POSITIVE_TRIG;
 126                break;
 127
 128        case IRQ_TYPE_LEVEL_LOW:
 129                it.trig_type = GPIO_LEVEL_TRIG;
 130                it.edge_type = GPIO_SINGLE_EDGE;
 131                it.logic_type = GPIO_NEGATIVE_TRIG;
 132                break;
 133
 134        default:
 135                return -EINVAL;
 136        }
 137
 138        eqbr_irq_type_cfg(&it, gctrl, offset);
 139        if (it.trig_type == GPIO_EDGE_TRIG)
 140                irq_set_handler_locked(d, handle_edge_irq);
 141        else
 142                irq_set_handler_locked(d, handle_level_irq);
 143
 144        return 0;
 145}
 146
 147static void eqbr_irq_handler(struct irq_desc *desc)
 148{
 149        struct gpio_chip *gc = irq_desc_get_handler_data(desc);
 150        struct eqbr_gpio_ctrl *gctrl = gpiochip_get_data(gc);
 151        struct irq_chip *ic = irq_desc_get_chip(desc);
 152        unsigned long pins, offset;
 153
 154        chained_irq_enter(ic, desc);
 155        pins = readl(gctrl->membase + GPIO_IRNCR);
 156
 157        for_each_set_bit(offset, &pins, gc->ngpio)
 158                generic_handle_irq(irq_find_mapping(gc->irq.domain, offset));
 159
 160        chained_irq_exit(ic, desc);
 161}
 162
 163static int gpiochip_setup(struct device *dev, struct eqbr_gpio_ctrl *gctrl)
 164{
 165        struct gpio_irq_chip *girq;
 166        struct gpio_chip *gc;
 167
 168        gc = &gctrl->chip;
 169        gc->label = gctrl->name;
 170#if defined(CONFIG_OF_GPIO)
 171        gc->of_node = gctrl->node;
 172#endif
 173
 174        if (!of_property_read_bool(gctrl->node, "interrupt-controller")) {
 175                dev_dbg(dev, "gc %s: doesn't act as interrupt controller!\n",
 176                        gctrl->name);
 177                return 0;
 178        }
 179
 180        gctrl->ic.name = "gpio_irq";
 181        gctrl->ic.irq_mask = eqbr_gpio_disable_irq;
 182        gctrl->ic.irq_unmask = eqbr_gpio_enable_irq;
 183        gctrl->ic.irq_ack = eqbr_gpio_ack_irq;
 184        gctrl->ic.irq_mask_ack = eqbr_gpio_mask_ack_irq;
 185        gctrl->ic.irq_set_type = eqbr_gpio_set_irq_type;
 186
 187        girq = &gctrl->chip.irq;
 188        girq->chip = &gctrl->ic;
 189        girq->parent_handler = eqbr_irq_handler;
 190        girq->num_parents = 1;
 191        girq->parents = devm_kcalloc(dev, 1, sizeof(*girq->parents), GFP_KERNEL);
 192        if (!girq->parents)
 193                return -ENOMEM;
 194
 195        girq->default_type = IRQ_TYPE_NONE;
 196        girq->handler = handle_bad_irq;
 197        girq->parents[0] = gctrl->virq;
 198
 199        return 0;
 200}
 201
 202static int gpiolib_reg(struct eqbr_pinctrl_drv_data *drvdata)
 203{
 204        struct device *dev = drvdata->dev;
 205        struct eqbr_gpio_ctrl *gctrl;
 206        struct device_node *np;
 207        struct resource res;
 208        int i, ret;
 209
 210        for (i = 0; i < drvdata->nr_gpio_ctrls; i++) {
 211                gctrl = drvdata->gpio_ctrls + i;
 212                np = gctrl->node;
 213
 214                gctrl->name = devm_kasprintf(dev, GFP_KERNEL, "gpiochip%d", i);
 215                if (!gctrl->name)
 216                        return -ENOMEM;
 217
 218                if (of_address_to_resource(np, 0, &res)) {
 219                        dev_err(dev, "Failed to get GPIO register address\n");
 220                        return -ENXIO;
 221                }
 222
 223                gctrl->membase = devm_ioremap_resource(dev, &res);
 224                if (IS_ERR(gctrl->membase))
 225                        return PTR_ERR(gctrl->membase);
 226
 227                gctrl->virq = irq_of_parse_and_map(np, 0);
 228                if (!gctrl->virq) {
 229                        dev_err(dev, "%s: failed to parse and map irq\n",
 230                                gctrl->name);
 231                        return -ENXIO;
 232                }
 233                raw_spin_lock_init(&gctrl->lock);
 234
 235                ret = bgpio_init(&gctrl->chip, dev, gctrl->bank->nr_pins / 8,
 236                                 gctrl->membase + GPIO_IN,
 237                                 gctrl->membase + GPIO_OUTSET,
 238                                 gctrl->membase + GPIO_OUTCLR,
 239                                 gctrl->membase + GPIO_DIR,
 240                                 NULL, 0);
 241                if (ret) {
 242                        dev_err(dev, "unable to init generic GPIO\n");
 243                        return ret;
 244                }
 245
 246                ret = gpiochip_setup(dev, gctrl);
 247                if (ret)
 248                        return ret;
 249
 250                ret = devm_gpiochip_add_data(dev, &gctrl->chip, gctrl);
 251                if (ret)
 252                        return ret;
 253        }
 254
 255        return 0;
 256}
 257
 258static inline struct eqbr_pin_bank
 259*find_pinbank_via_pin(struct eqbr_pinctrl_drv_data *pctl, unsigned int pin)
 260{
 261        struct eqbr_pin_bank *bank;
 262        int i;
 263
 264        for (i = 0; i < pctl->nr_banks; i++) {
 265                bank = &pctl->pin_banks[i];
 266                if (pin >= bank->pin_base &&
 267                    (pin - bank->pin_base) < bank->nr_pins)
 268                        return bank;
 269        }
 270
 271        return NULL;
 272}
 273
 274static const struct pinctrl_ops eqbr_pctl_ops = {
 275        .get_groups_count       = pinctrl_generic_get_group_count,
 276        .get_group_name         = pinctrl_generic_get_group_name,
 277        .get_group_pins         = pinctrl_generic_get_group_pins,
 278        .dt_node_to_map         = pinconf_generic_dt_node_to_map_all,
 279        .dt_free_map            = pinconf_generic_dt_free_map,
 280};
 281
 282static int eqbr_set_pin_mux(struct eqbr_pinctrl_drv_data *pctl,
 283                            unsigned int pmx, unsigned int pin)
 284{
 285        struct eqbr_pin_bank *bank;
 286        unsigned long flags;
 287        unsigned int offset;
 288        void __iomem *mem;
 289
 290        bank = find_pinbank_via_pin(pctl, pin);
 291        if (!bank) {
 292                dev_err(pctl->dev, "Couldn't find pin bank for pin %u\n", pin);
 293                return -ENODEV;
 294        }
 295        mem = bank->membase;
 296        offset = pin - bank->pin_base;
 297
 298        if (!(bank->aval_pinmap & BIT(offset))) {
 299                dev_err(pctl->dev,
 300                        "PIN: %u is not valid, pinbase: %u, bitmap: %u\n",
 301                        pin, bank->pin_base, bank->aval_pinmap);
 302                return -ENODEV;
 303        }
 304
 305        raw_spin_lock_irqsave(&pctl->lock, flags);
 306        writel(pmx, mem + (offset * 4));
 307        raw_spin_unlock_irqrestore(&pctl->lock, flags);
 308        return 0;
 309}
 310
 311static int eqbr_pinmux_set_mux(struct pinctrl_dev *pctldev,
 312                               unsigned int selector, unsigned int group)
 313{
 314        struct eqbr_pinctrl_drv_data *pctl = pinctrl_dev_get_drvdata(pctldev);
 315        struct function_desc *func;
 316        struct group_desc *grp;
 317        unsigned int *pinmux;
 318        int i;
 319
 320        func = pinmux_generic_get_function(pctldev, selector);
 321        if (!func)
 322                return -EINVAL;
 323
 324        grp = pinctrl_generic_get_group(pctldev, group);
 325        if (!grp)
 326                return -EINVAL;
 327
 328        pinmux = grp->data;
 329        for (i = 0; i < grp->num_pins; i++)
 330                eqbr_set_pin_mux(pctl, pinmux[i], grp->pins[i]);
 331
 332        return 0;
 333}
 334
 335static int eqbr_pinmux_gpio_request(struct pinctrl_dev *pctldev,
 336                                    struct pinctrl_gpio_range *range,
 337                                    unsigned int pin)
 338{
 339        struct eqbr_pinctrl_drv_data *pctl = pinctrl_dev_get_drvdata(pctldev);
 340
 341        return eqbr_set_pin_mux(pctl, EQBR_GPIO_MODE, pin);
 342}
 343
 344static const struct pinmux_ops eqbr_pinmux_ops = {
 345        .get_functions_count    = pinmux_generic_get_function_count,
 346        .get_function_name      = pinmux_generic_get_function_name,
 347        .get_function_groups    = pinmux_generic_get_function_groups,
 348        .set_mux                = eqbr_pinmux_set_mux,
 349        .gpio_request_enable    = eqbr_pinmux_gpio_request,
 350        .strict                 = true,
 351};
 352
 353static int get_drv_cur(void __iomem *mem, unsigned int offset)
 354{
 355        unsigned int idx = offset / DRV_CUR_PINS; /* 0-15, 16-31 per register*/
 356        unsigned int pin_offset = offset % DRV_CUR_PINS;
 357
 358        return PARSE_DRV_CURRENT(readl(mem + REG_DRCC(idx)), pin_offset);
 359}
 360
 361static struct eqbr_gpio_ctrl
 362*get_gpio_ctrls_via_bank(struct eqbr_pinctrl_drv_data *pctl,
 363                        struct eqbr_pin_bank *bank)
 364{
 365        int i;
 366
 367        for (i = 0; i < pctl->nr_gpio_ctrls; i++) {
 368                if (pctl->gpio_ctrls[i].bank == bank)
 369                        return &pctl->gpio_ctrls[i];
 370        }
 371
 372        return NULL;
 373}
 374
 375static int eqbr_pinconf_get(struct pinctrl_dev *pctldev, unsigned int pin,
 376                            unsigned long *config)
 377{
 378        struct eqbr_pinctrl_drv_data *pctl = pinctrl_dev_get_drvdata(pctldev);
 379        enum pin_config_param param = pinconf_to_config_param(*config);
 380        struct eqbr_gpio_ctrl *gctrl;
 381        struct eqbr_pin_bank *bank;
 382        unsigned long flags;
 383        unsigned int offset;
 384        void __iomem *mem;
 385        u32 val;
 386
 387        bank = find_pinbank_via_pin(pctl, pin);
 388        if (!bank) {
 389                dev_err(pctl->dev, "Couldn't find pin bank for pin %u\n", pin);
 390                return -ENODEV;
 391        }
 392        mem = bank->membase;
 393        offset = pin - bank->pin_base;
 394
 395        if (!(bank->aval_pinmap & BIT(offset))) {
 396                dev_err(pctl->dev,
 397                        "PIN: %u is not valid, pinbase: %u, bitmap: %u\n",
 398                        pin, bank->pin_base, bank->aval_pinmap);
 399                return -ENODEV;
 400        }
 401
 402        raw_spin_lock_irqsave(&pctl->lock, flags);
 403        switch (param) {
 404        case PIN_CONFIG_BIAS_PULL_UP:
 405                val = !!(readl(mem + REG_PUEN) & BIT(offset));
 406                break;
 407        case PIN_CONFIG_BIAS_PULL_DOWN:
 408                val = !!(readl(mem + REG_PDEN) & BIT(offset));
 409                break;
 410        case PIN_CONFIG_DRIVE_OPEN_DRAIN:
 411                val = !!(readl(mem + REG_OD) & BIT(offset));
 412                break;
 413        case PIN_CONFIG_DRIVE_STRENGTH:
 414                val = get_drv_cur(mem, offset);
 415                break;
 416        case PIN_CONFIG_SLEW_RATE:
 417                val = !!(readl(mem + REG_SRC) & BIT(offset));
 418                break;
 419        case PIN_CONFIG_OUTPUT_ENABLE:
 420                gctrl = get_gpio_ctrls_via_bank(pctl, bank);
 421                if (!gctrl) {
 422                        dev_err(pctl->dev, "Failed to find gpio via bank pinbase: %u, pin: %u\n",
 423                                bank->pin_base, pin);
 424                        raw_spin_unlock_irqrestore(&pctl->lock, flags);
 425                        return -ENODEV;
 426                }
 427                val = !!(readl(gctrl->membase + GPIO_DIR) & BIT(offset));
 428                break;
 429        default:
 430                raw_spin_unlock_irqrestore(&pctl->lock, flags);
 431                return -ENOTSUPP;
 432        }
 433        raw_spin_unlock_irqrestore(&pctl->lock, flags);
 434        *config = pinconf_to_config_packed(param, val);
 435;
 436        return 0;
 437}
 438
 439static int eqbr_pinconf_set(struct pinctrl_dev *pctldev, unsigned int pin,
 440                            unsigned long *configs, unsigned int num_configs)
 441{
 442        struct eqbr_pinctrl_drv_data *pctl = pinctrl_dev_get_drvdata(pctldev);
 443        struct eqbr_gpio_ctrl *gctrl;
 444        enum pin_config_param param;
 445        struct eqbr_pin_bank *bank;
 446        unsigned int val, offset;
 447        struct gpio_chip *gc;
 448        unsigned long flags;
 449        void __iomem *mem;
 450        u32 regval, mask;
 451        int i;
 452
 453        for (i = 0; i < num_configs; i++) {
 454                param = pinconf_to_config_param(configs[i]);
 455                val = pinconf_to_config_argument(configs[i]);
 456
 457                bank = find_pinbank_via_pin(pctl, pin);
 458                if (!bank) {
 459                        dev_err(pctl->dev,
 460                                "Couldn't find pin bank for pin %u\n", pin);
 461                        return -ENODEV;
 462                }
 463                mem = bank->membase;
 464                offset = pin - bank->pin_base;
 465
 466                switch (param) {
 467                case PIN_CONFIG_BIAS_PULL_UP:
 468                        mem += REG_PUEN;
 469                        mask = BIT(offset);
 470                        break;
 471                case PIN_CONFIG_BIAS_PULL_DOWN:
 472                        mem += REG_PDEN;
 473                        mask = BIT(offset);
 474                        break;
 475                case PIN_CONFIG_DRIVE_OPEN_DRAIN:
 476                        mem += REG_OD;
 477                        mask = BIT(offset);
 478                        break;
 479                case PIN_CONFIG_DRIVE_STRENGTH:
 480                        mem += REG_DRCC(offset / DRV_CUR_PINS);
 481                        offset = (offset % DRV_CUR_PINS) * 2;
 482                        mask = GENMASK(1, 0) << offset;
 483                        break;
 484                case PIN_CONFIG_SLEW_RATE:
 485                        mem += REG_SRC;
 486                        mask = BIT(offset);
 487                        break;
 488                case PIN_CONFIG_OUTPUT_ENABLE:
 489                        gctrl = get_gpio_ctrls_via_bank(pctl, bank);
 490                        if (!gctrl) {
 491                                dev_err(pctl->dev, "Failed to find gpio via bank pinbase: %u, pin: %u\n",
 492                                        bank->pin_base, pin);
 493                                return -ENODEV;
 494                        }
 495                        gc = &gctrl->chip;
 496                        gc->direction_output(gc, offset, 0);
 497                        continue;
 498                default:
 499                        return -ENOTSUPP;
 500                }
 501
 502                raw_spin_lock_irqsave(&pctl->lock, flags);
 503                regval = readl(mem);
 504                regval = (regval & ~mask) | ((val << offset) & mask);
 505                writel(regval, mem);
 506                raw_spin_unlock_irqrestore(&pctl->lock, flags);
 507        }
 508
 509        return 0;
 510}
 511
 512static int eqbr_pinconf_group_get(struct pinctrl_dev *pctldev,
 513                                  unsigned int group, unsigned long *config)
 514{
 515        unsigned int i, npins, old = 0;
 516        const unsigned int *pins;
 517        int ret;
 518
 519        ret = pinctrl_generic_get_group_pins(pctldev, group, &pins, &npins);
 520        if (ret)
 521                return ret;
 522
 523        for (i = 0; i < npins; i++) {
 524                if (eqbr_pinconf_get(pctldev, pins[i], config))
 525                        return -ENOTSUPP;
 526
 527                if (i && old != *config)
 528                        return -ENOTSUPP;
 529
 530                old = *config;
 531        }
 532        return 0;
 533}
 534
 535static int eqbr_pinconf_group_set(struct pinctrl_dev *pctldev,
 536                                  unsigned int group, unsigned long *configs,
 537                                  unsigned int num_configs)
 538{
 539        const unsigned int *pins;
 540        unsigned int i, npins;
 541        int ret;
 542
 543        ret = pinctrl_generic_get_group_pins(pctldev, group, &pins, &npins);
 544        if (ret)
 545                return ret;
 546
 547        for (i = 0; i < npins; i++) {
 548                ret = eqbr_pinconf_set(pctldev, pins[i], configs, num_configs);
 549                if (ret)
 550                        return ret;
 551        }
 552        return 0;
 553}
 554
 555static const struct pinconf_ops eqbr_pinconf_ops = {
 556        .is_generic                     = true,
 557        .pin_config_get                 = eqbr_pinconf_get,
 558        .pin_config_set                 = eqbr_pinconf_set,
 559        .pin_config_group_get           = eqbr_pinconf_group_get,
 560        .pin_config_group_set           = eqbr_pinconf_group_set,
 561        .pin_config_config_dbg_show     = pinconf_generic_dump_config,
 562};
 563
 564static bool is_func_exist(struct eqbr_pmx_func *funcs, const char *name,
 565                         unsigned int nr_funcs, unsigned int *idx)
 566{
 567        int i;
 568
 569        if (!funcs)
 570                return false;
 571
 572        for (i = 0; i < nr_funcs; i++) {
 573                if (funcs[i].name && !strcmp(funcs[i].name, name)) {
 574                        *idx = i;
 575                        return true;
 576                }
 577        }
 578
 579        return false;
 580}
 581
 582static int funcs_utils(struct device *dev, struct eqbr_pmx_func *funcs,
 583                       unsigned int *nr_funcs, funcs_util_ops op)
 584{
 585        struct device_node *node = dev->of_node;
 586        struct device_node *np;
 587        struct property *prop;
 588        const char *fn_name;
 589        unsigned int fid;
 590        int i, j;
 591
 592        i = 0;
 593        for_each_child_of_node(node, np) {
 594                prop = of_find_property(np, "groups", NULL);
 595                if (!prop)
 596                        continue;
 597
 598                if (of_property_read_string(np, "function", &fn_name)) {
 599                        /* some groups may not have function, it's OK */
 600                        dev_dbg(dev, "Group %s: not function binded!\n",
 601                                (char *)prop->value);
 602                        continue;
 603                }
 604
 605                switch (op) {
 606                case OP_COUNT_NR_FUNCS:
 607                        if (!is_func_exist(funcs, fn_name, *nr_funcs, &fid))
 608                                *nr_funcs = *nr_funcs + 1;
 609                        break;
 610
 611                case OP_ADD_FUNCS:
 612                        if (!is_func_exist(funcs, fn_name, *nr_funcs, &fid))
 613                                funcs[i].name = fn_name;
 614                        break;
 615
 616                case OP_COUNT_NR_FUNC_GRPS:
 617                        if (is_func_exist(funcs, fn_name, *nr_funcs, &fid))
 618                                funcs[fid].nr_groups++;
 619                        break;
 620
 621                case OP_ADD_FUNC_GRPS:
 622                        if (is_func_exist(funcs, fn_name, *nr_funcs, &fid)) {
 623                                for (j = 0; j < funcs[fid].nr_groups; j++)
 624                                        if (!funcs[fid].groups[j])
 625                                                break;
 626                                funcs[fid].groups[j] = prop->value;
 627                        }
 628                        break;
 629
 630                default:
 631                        of_node_put(np);
 632                        return -EINVAL;
 633                }
 634                i++;
 635        }
 636
 637        return 0;
 638}
 639
 640static int eqbr_build_functions(struct eqbr_pinctrl_drv_data *drvdata)
 641{
 642        struct device *dev = drvdata->dev;
 643        struct eqbr_pmx_func *funcs = NULL;
 644        unsigned int nr_funcs = 0;
 645        int i, ret;
 646
 647        ret = funcs_utils(dev, funcs, &nr_funcs, OP_COUNT_NR_FUNCS);
 648        if (ret)
 649                return ret;
 650
 651        funcs = devm_kcalloc(dev, nr_funcs, sizeof(*funcs), GFP_KERNEL);
 652        if (!funcs)
 653                return -ENOMEM;
 654
 655        ret = funcs_utils(dev, funcs, &nr_funcs, OP_ADD_FUNCS);
 656        if (ret)
 657                return ret;
 658
 659        ret = funcs_utils(dev, funcs, &nr_funcs, OP_COUNT_NR_FUNC_GRPS);
 660        if (ret)
 661                return ret;
 662
 663        for (i = 0; i < nr_funcs; i++) {
 664                if (!funcs[i].nr_groups)
 665                        continue;
 666                funcs[i].groups = devm_kcalloc(dev, funcs[i].nr_groups,
 667                                               sizeof(*(funcs[i].groups)),
 668                                               GFP_KERNEL);
 669                if (!funcs[i].groups)
 670                        return -ENOMEM;
 671        }
 672
 673        ret = funcs_utils(dev, funcs, &nr_funcs, OP_ADD_FUNC_GRPS);
 674        if (ret)
 675                return ret;
 676
 677        for (i = 0; i < nr_funcs; i++) {
 678                ret = pinmux_generic_add_function(drvdata->pctl_dev,
 679                                                  funcs[i].name,
 680                                                  funcs[i].groups,
 681                                                  funcs[i].nr_groups,
 682                                                  drvdata);
 683                if (ret < 0) {
 684                        dev_err(dev, "Failed to register function %s\n",
 685                                funcs[i].name);
 686                        return ret;
 687                }
 688        }
 689
 690        return 0;
 691}
 692
 693static int eqbr_build_groups(struct eqbr_pinctrl_drv_data *drvdata)
 694{
 695        struct device *dev = drvdata->dev;
 696        struct device_node *node = dev->of_node;
 697        unsigned int *pinmux, pin_id, pinmux_id;
 698        struct group_desc group;
 699        struct device_node *np;
 700        struct property *prop;
 701        int j, err;
 702
 703        for_each_child_of_node(node, np) {
 704                prop = of_find_property(np, "groups", NULL);
 705                if (!prop)
 706                        continue;
 707
 708                group.num_pins = of_property_count_u32_elems(np, "pins");
 709                if (group.num_pins < 0) {
 710                        dev_err(dev, "No pins in the group: %s\n", prop->name);
 711                        of_node_put(np);
 712                        return -EINVAL;
 713                }
 714                group.name = prop->value;
 715                group.pins = devm_kcalloc(dev, group.num_pins,
 716                                          sizeof(*(group.pins)), GFP_KERNEL);
 717                if (!group.pins) {
 718                        of_node_put(np);
 719                        return -ENOMEM;
 720                }
 721
 722                pinmux = devm_kcalloc(dev, group.num_pins, sizeof(*pinmux),
 723                                      GFP_KERNEL);
 724                if (!pinmux) {
 725                        of_node_put(np);
 726                        return -ENOMEM;
 727                }
 728
 729                for (j = 0; j < group.num_pins; j++) {
 730                        if (of_property_read_u32_index(np, "pins", j, &pin_id)) {
 731                                dev_err(dev, "Group %s: Read intel pins id failed\n",
 732                                        group.name);
 733                                of_node_put(np);
 734                                return -EINVAL;
 735                        }
 736                        if (pin_id >= drvdata->pctl_desc.npins) {
 737                                dev_err(dev, "Group %s: Invalid pin ID, idx: %d, pin %u\n",
 738                                        group.name, j, pin_id);
 739                                of_node_put(np);
 740                                return -EINVAL;
 741                        }
 742                        group.pins[j] = pin_id;
 743                        if (of_property_read_u32_index(np, "pinmux", j, &pinmux_id)) {
 744                                dev_err(dev, "Group %s: Read intel pinmux id failed\n",
 745                                        group.name);
 746                                of_node_put(np);
 747                                return -EINVAL;
 748                        }
 749                        pinmux[j] = pinmux_id;
 750                }
 751
 752                err = pinctrl_generic_add_group(drvdata->pctl_dev, group.name,
 753                                                group.pins, group.num_pins,
 754                                                pinmux);
 755                if (err < 0) {
 756                        dev_err(dev, "Failed to register group %s\n", group.name);
 757                        of_node_put(np);
 758                        return err;
 759                }
 760                memset(&group, 0, sizeof(group));
 761                pinmux = NULL;
 762        }
 763
 764        return 0;
 765}
 766
 767static int pinctrl_reg(struct eqbr_pinctrl_drv_data *drvdata)
 768{
 769        struct pinctrl_desc *pctl_desc;
 770        struct pinctrl_pin_desc *pdesc;
 771        struct device *dev;
 772        unsigned int nr_pins;
 773        char *pin_names;
 774        int i, ret;
 775
 776        dev = drvdata->dev;
 777        pctl_desc = &drvdata->pctl_desc;
 778        pctl_desc->name = "eqbr-pinctrl";
 779        pctl_desc->owner = THIS_MODULE;
 780        pctl_desc->pctlops = &eqbr_pctl_ops;
 781        pctl_desc->pmxops = &eqbr_pinmux_ops;
 782        pctl_desc->confops = &eqbr_pinconf_ops;
 783        raw_spin_lock_init(&drvdata->lock);
 784
 785        for (i = 0, nr_pins = 0; i < drvdata->nr_banks; i++)
 786                nr_pins += drvdata->pin_banks[i].nr_pins;
 787
 788        pdesc = devm_kcalloc(dev, nr_pins, sizeof(*pdesc), GFP_KERNEL);
 789        if (!pdesc)
 790                return -ENOMEM;
 791        pin_names = devm_kcalloc(dev, nr_pins, PIN_NAME_LEN, GFP_KERNEL);
 792        if (!pin_names)
 793                return -ENOMEM;
 794
 795        for (i = 0; i < nr_pins; i++) {
 796                sprintf(pin_names, PIN_NAME_FMT, i);
 797                pdesc[i].number = i;
 798                pdesc[i].name = pin_names;
 799                pin_names += PIN_NAME_LEN;
 800        }
 801        pctl_desc->pins = pdesc;
 802        pctl_desc->npins = nr_pins;
 803        dev_dbg(dev, "pinctrl total pin number: %u\n", nr_pins);
 804
 805        ret = devm_pinctrl_register_and_init(dev, pctl_desc, drvdata,
 806                                             &drvdata->pctl_dev);
 807        if (ret)
 808                return ret;
 809
 810        ret = eqbr_build_groups(drvdata);
 811        if (ret) {
 812                dev_err(dev, "Failed to build groups\n");
 813                return ret;
 814        }
 815
 816        ret = eqbr_build_functions(drvdata);
 817        if (ret) {
 818                dev_err(dev, "Failed to build groups\n");
 819                return ret;
 820        }
 821
 822        return pinctrl_enable(drvdata->pctl_dev);
 823}
 824
 825static int pinbank_init(struct device_node *np,
 826                        struct eqbr_pinctrl_drv_data *drvdata,
 827                        struct eqbr_pin_bank *bank, unsigned int id)
 828{
 829        struct device *dev = drvdata->dev;
 830        struct of_phandle_args spec;
 831        int ret;
 832
 833        bank->membase = drvdata->membase + id * PAD_REG_OFF;
 834
 835        ret = of_parse_phandle_with_fixed_args(np, "gpio-ranges", 3, 0, &spec);
 836        if (ret) {
 837                dev_err(dev, "gpio-range not available!\n");
 838                return ret;
 839        }
 840
 841        bank->pin_base = spec.args[1];
 842        bank->nr_pins = spec.args[2];
 843
 844        bank->aval_pinmap = readl(bank->membase + REG_AVAIL);
 845        bank->id = id;
 846
 847        dev_dbg(dev, "pinbank id: %d, reg: %px, pinbase: %u, pin number: %u, pinmap: 0x%x\n",
 848                id, bank->membase, bank->pin_base,
 849                bank->nr_pins, bank->aval_pinmap);
 850
 851        return ret;
 852}
 853
 854static int pinbank_probe(struct eqbr_pinctrl_drv_data *drvdata)
 855{
 856        struct device *dev = drvdata->dev;
 857        struct device_node *np_gpio;
 858        struct eqbr_gpio_ctrl *gctrls;
 859        struct eqbr_pin_bank *banks;
 860        int i, nr_gpio;
 861
 862        /* Count gpio bank number */
 863        nr_gpio = 0;
 864        for_each_node_by_name(np_gpio, "gpio") {
 865                if (of_device_is_available(np_gpio))
 866                        nr_gpio++;
 867        }
 868
 869        if (!nr_gpio) {
 870                dev_err(dev, "NO pin bank available!\n");
 871                return -ENODEV;
 872        }
 873
 874        /* Count pin bank number and gpio controller number */
 875        banks = devm_kcalloc(dev, nr_gpio, sizeof(*banks), GFP_KERNEL);
 876        if (!banks)
 877                return -ENOMEM;
 878
 879        gctrls = devm_kcalloc(dev, nr_gpio, sizeof(*gctrls), GFP_KERNEL);
 880        if (!gctrls)
 881                return -ENOMEM;
 882
 883        dev_dbg(dev, "found %d gpio controller!\n", nr_gpio);
 884
 885        /* Initialize Pin bank */
 886        i = 0;
 887        for_each_node_by_name(np_gpio, "gpio") {
 888                if (!of_device_is_available(np_gpio))
 889                        continue;
 890
 891                pinbank_init(np_gpio, drvdata, banks + i, i);
 892
 893                gctrls[i].node = np_gpio;
 894                gctrls[i].bank = banks + i;
 895                i++;
 896        }
 897
 898        drvdata->pin_banks = banks;
 899        drvdata->nr_banks = nr_gpio;
 900        drvdata->gpio_ctrls = gctrls;
 901        drvdata->nr_gpio_ctrls = nr_gpio;
 902
 903        return 0;
 904}
 905
 906static int eqbr_pinctrl_probe(struct platform_device *pdev)
 907{
 908        struct eqbr_pinctrl_drv_data *drvdata;
 909        struct device *dev = &pdev->dev;
 910        int ret;
 911
 912        drvdata = devm_kzalloc(dev, sizeof(*drvdata), GFP_KERNEL);
 913        if (!drvdata)
 914                return -ENOMEM;
 915
 916        drvdata->dev = dev;
 917
 918        drvdata->membase = devm_platform_ioremap_resource(pdev, 0);
 919        if (IS_ERR(drvdata->membase))
 920                return PTR_ERR(drvdata->membase);
 921
 922        ret = pinbank_probe(drvdata);
 923        if (ret)
 924                return ret;
 925
 926        ret = pinctrl_reg(drvdata);
 927        if (ret)
 928                return ret;
 929
 930        ret = gpiolib_reg(drvdata);
 931        if (ret)
 932                return ret;
 933
 934        platform_set_drvdata(pdev, drvdata);
 935        return 0;
 936}
 937
 938static const struct of_device_id eqbr_pinctrl_dt_match[] = {
 939        { .compatible = "intel,lgm-io" },
 940        {}
 941};
 942MODULE_DEVICE_TABLE(of, eqbr_pinctrl_dt_match);
 943
 944static struct platform_driver eqbr_pinctrl_driver = {
 945        .probe  = eqbr_pinctrl_probe,
 946        .driver = {
 947                .name = "eqbr-pinctrl",
 948                .of_match_table = eqbr_pinctrl_dt_match,
 949        },
 950};
 951
 952module_platform_driver(eqbr_pinctrl_driver);
 953
 954MODULE_AUTHOR("Zhu Yixin <yixin.zhu@intel.com>, Rahul Tanwar <rahul.tanwar@intel.com>");
 955MODULE_DESCRIPTION("Pinctrl Driver for LGM SoC (Equilibrium)");
 956MODULE_LICENSE("GPL v2");
 957