drivers/pinctrl/pinctrl-equilibrium.c

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