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->grp.npins; i++)
 335                 eqbr_set_pin_mux(pctl, pinmux[i], grp->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 pinfunction *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 pinfunction *funcs,
 588                        unsigned int *nr_funcs, funcs_util_ops op)
 589 {
 590         struct device_node *node = dev->of_node;
 591         struct property *prop;
 592         const char *fn_name;
 593         const char **groups;
 594         unsigned int fid;
 595         int i, j;
 596
 597         i = 0;
 598         for_each_child_of_node_scoped(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].ngroups++;
 624                         break;
 625
 626                 case OP_ADD_FUNC_GRPS:
 627                         if (is_func_exist(funcs, fn_name, *nr_funcs, &fid)) {
 628                                 groups = (const char **)funcs[fid].groups;
 629                                 for (j = 0; j < funcs[fid].ngroups; j++)
 630                                         if (!groups[j])
 631                                                 break;
 632                                 groups[j] = prop->value;
 633                         }
 634                         break;
 635
 636                 default:
 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 pinfunction *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].ngroups)
 670                         continue;
 671                 funcs[i].groups = devm_kcalloc(dev, funcs[i].ngroups,
 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].ngroups,
 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 *pins, *pinmux, pin_id, pinmux_id;
 708         struct pingroup group, *grp = &group;
 709         struct property *prop;
 710         int j, err;
 711
 712         for_each_child_of_node_scoped(node, np) {
 713                 prop = of_find_property(np, "groups", NULL);
 714                 if (!prop)
 715                         continue;
 716
 717                 err = of_property_count_u32_elems(np, "pins");
 718                 if (err < 0) {
 719                         dev_err(dev, "No pins in the group: %s\n", prop->name);
 720                         return err;
 721                 }
 722                 grp->npins = err;
 723                 grp->name = prop->value;
 724                 pins = devm_kcalloc(dev, grp->npins, sizeof(*pins), GFP_KERNEL);
 725                 if (!pins)
 726                         return -ENOMEM;
 727
 728                 grp->pins = pins;
 729
 730                 pinmux = devm_kcalloc(dev, grp->npins, sizeof(*pinmux), GFP_KERNEL);
 731                 if (!pinmux)
 732                         return -ENOMEM;
 733
 734                 for (j = 0; j < grp->npins; j++) {
 735                         if (of_property_read_u32_index(np, "pins", j, &pin_id)) {
 736                                 dev_err(dev, "Group %s: Read intel pins id failed\n",
 737                                         grp->name);
 738                                 return -EINVAL;
 739                         }
 740                         if (pin_id >= drvdata->pctl_desc.npins) {
 741                                 dev_err(dev, "Group %s: Invalid pin ID, idx: %d, pin %u\n",
 742                                         grp->name, j, pin_id);
 743                                 return -EINVAL;
 744                         }
 745                         pins[j] = pin_id;
 746                         if (of_property_read_u32_index(np, "pinmux", j, &pinmux_id)) {
 747                                 dev_err(dev, "Group %s: Read intel pinmux id failed\n",
 748                                         grp->name);
 749                                 return -EINVAL;
 750                         }
 751                         pinmux[j] = pinmux_id;
 752                 }
 753
 754                 err = pinctrl_generic_add_group(drvdata->pctl_dev,
 755                                                 grp->name, grp->pins, grp->npins,
 756                                                 pinmux);
 757                 if (err < 0) {
 758                         dev_err(dev, "Failed to register group %s\n", grp->name);
 759                         return err;
 760                 }
 761                 memset(&group, 0, sizeof(group));
 762                 pinmux = NULL;
 763         }
 764
 765         return 0;
 766 }
 767
 768 static int pinctrl_reg(struct eqbr_pinctrl_drv_data *drvdata)
 769 {
 770         struct pinctrl_desc *pctl_desc;
 771         struct pinctrl_pin_desc *pdesc;
 772         struct device *dev;
 773         unsigned int nr_pins;
 774         char *pin_names;
 775         int i, ret;
 776
 777         dev = drvdata->dev;
 778         pctl_desc = &drvdata->pctl_desc;
 779         pctl_desc->name = "eqbr-pinctrl";
 780         pctl_desc->owner = THIS_MODULE;
 781         pctl_desc->pctlops = &eqbr_pctl_ops;
 782         pctl_desc->pmxops = &eqbr_pinmux_ops;
 783         pctl_desc->confops = &eqbr_pinconf_ops;
 784         raw_spin_lock_init(&drvdata->lock);
 785
 786         for (i = 0, nr_pins = 0; i < drvdata->nr_banks; i++)
 787                 nr_pins += drvdata->pin_banks[i].nr_pins;
 788
 789         pdesc = devm_kcalloc(dev, nr_pins, sizeof(*pdesc), GFP_KERNEL);
 790         if (!pdesc)
 791                 return -ENOMEM;
 792         pin_names = devm_kcalloc(dev, nr_pins, PIN_NAME_LEN, GFP_KERNEL);
 793         if (!pin_names)
 794                 return -ENOMEM;
 795
 796         for (i = 0; i < nr_pins; i++) {
 797                 sprintf(pin_names, PIN_NAME_FMT, i);
 798                 pdesc[i].number = i;
 799                 pdesc[i].name = pin_names;
 800                 pin_names += PIN_NAME_LEN;
 801         }
 802         pctl_desc->pins = pdesc;
 803         pctl_desc->npins = nr_pins;
 804         dev_dbg(dev, "pinctrl total pin number: %u\n", nr_pins);
 805
 806         ret = devm_pinctrl_register_and_init(dev, pctl_desc, drvdata,
 807                                              &drvdata->pctl_dev);
 808         if (ret)
 809                 return ret;
 810
 811         ret = eqbr_build_groups(drvdata);
 812         if (ret) {
 813                 dev_err(dev, "Failed to build groups\n");
 814                 return ret;
 815         }
 816
 817         ret = eqbr_build_functions(drvdata);
 818         if (ret) {
 819                 dev_err(dev, "Failed to build functions\n");
 820                 return ret;
 821         }
 822
 823         return pinctrl_enable(drvdata->pctl_dev);
 824 }
 825
 826 static int pinbank_init(struct device_node *np,
 827                         struct eqbr_pinctrl_drv_data *drvdata,
 828                         struct eqbr_pin_bank *bank, unsigned int id)
 829 {
 830         struct device *dev = drvdata->dev;
 831         struct of_phandle_args spec;
 832         int ret;
 833
 834         bank->membase = drvdata->membase + id * PAD_REG_OFF;
 835
 836         ret = of_parse_phandle_with_fixed_args(np, "gpio-ranges", 3, 0, &spec);
 837         if (ret) {
 838                 dev_err(dev, "gpio-range not available!\n");
 839                 return ret;
 840         }
 841
 842         bank->pin_base = spec.args[1];
 843         bank->nr_pins = spec.args[2];
 844
 845         bank->aval_pinmap = readl(bank->membase + REG_AVAIL);
 846         bank->id = id;
 847
 848         dev_dbg(dev, "pinbank id: %d, reg: %px, pinbase: %u, pin number: %u, pinmap: 0x%x\n",
 849                 id, bank->membase, bank->pin_base,
 850                 bank->nr_pins, bank->aval_pinmap);
 851
 852         return ret;
 853 }
 854
 855 static int pinbank_probe(struct eqbr_pinctrl_drv_data *drvdata)
 856 {
 857         struct device *dev = drvdata->dev;
 858         struct device_node *np_gpio;
 859         struct eqbr_gpio_ctrl *gctrls;
 860         struct eqbr_pin_bank *banks;
 861         int i, nr_gpio;
 862
 863         /* Count gpio bank number */
 864         nr_gpio = 0;
 865         for_each_node_by_name(np_gpio, "gpio") {
 866                 if (of_device_is_available(np_gpio))
 867                         nr_gpio++;
 868         }
 869
 870         if (!nr_gpio) {
 871                 dev_err(dev, "NO pin bank available!\n");
 872                 return -ENODEV;
 873         }
 874
 875         /* Count pin bank number and gpio controller number */
 876         banks = devm_kcalloc(dev, nr_gpio, sizeof(*banks), GFP_KERNEL);
 877         if (!banks)
 878                 return -ENOMEM;
 879
 880         gctrls = devm_kcalloc(dev, nr_gpio, sizeof(*gctrls), GFP_KERNEL);
 881         if (!gctrls)
 882                 return -ENOMEM;
 883
 884         dev_dbg(dev, "found %d gpio controller!\n", nr_gpio);
 885
 886         /* Initialize Pin bank */
 887         i = 0;
 888         for_each_node_by_name(np_gpio, "gpio") {
 889                 if (!of_device_is_available(np_gpio))
 890                         continue;
 891
 892                 pinbank_init(np_gpio, drvdata, banks + i, i);
 893
 894                 gctrls[i].fwnode = of_fwnode_handle(np_gpio);
 895                 gctrls[i].bank = banks + i;
 896                 i++;
 897         }
 898
 899         drvdata->pin_banks = banks;
 900         drvdata->nr_banks = nr_gpio;
 901         drvdata->gpio_ctrls = gctrls;
 902         drvdata->nr_gpio_ctrls = nr_gpio;
 903
 904         return 0;
 905 }
 906
 907 static int eqbr_pinctrl_probe(struct platform_device *pdev)
 908 {
 909         struct eqbr_pinctrl_drv_data *drvdata;
 910         struct device *dev = &pdev->dev;
 911         int ret;
 912
 913         drvdata = devm_kzalloc(dev, sizeof(*drvdata), GFP_KERNEL);
 914         if (!drvdata)
 915                 return -ENOMEM;
 916
 917         drvdata->dev = dev;
 918
 919         drvdata->membase = devm_platform_ioremap_resource(pdev, 0);
 920         if (IS_ERR(drvdata->membase))
 921                 return PTR_ERR(drvdata->membase);
 922
 923         ret = pinbank_probe(drvdata);
 924         if (ret)
 925                 return ret;
 926
 927         ret = pinctrl_reg(drvdata);
 928         if (ret)
 929                 return ret;
 930
 931         ret = gpiolib_reg(drvdata);
 932         if (ret)
 933                 return ret;
 934
 935         platform_set_drvdata(pdev, drvdata);
 936         return 0;
 937 }
 938
 939 static const struct of_device_id eqbr_pinctrl_dt_match[] = {
 940         { .compatible = "intel,lgm-io" },
 941         {}
 942 };
 943 MODULE_DEVICE_TABLE(of, eqbr_pinctrl_dt_match);
 944
 945 static struct platform_driver eqbr_pinctrl_driver = {
 946         .probe  = eqbr_pinctrl_probe,
 947         .driver = {
 948                 .name = "eqbr-pinctrl",
 949                 .of_match_table = eqbr_pinctrl_dt_match,
 950         },
 951 };
 952
 953 module_platform_driver(eqbr_pinctrl_driver);
 954
 955 MODULE_AUTHOR("Zhu Yixin <yixin.zhu@intel.com>, Rahul Tanwar <rahul.tanwar@intel.com>");
 956 MODULE_DESCRIPTION("Pinctrl Driver for LGM SoC (Equilibrium)");
 957 MODULE_LICENSE("GPL v2");