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
  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
  24 static 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
  36 static 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
  49 static 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
  61 static 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
  67 static 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
  76 static 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
  91 static 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
 147 static 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_domain_irq(gc->irq.domain, offset);
 159
 160         chained_irq_exit(ic, desc);
 161 }
 162
 163 static 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
 202 static 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
 258 static 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
 274 static 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
 282 static 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
 311 static 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
 335 static 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
 344 static 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
 353 static 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
 361 static 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
 375 static 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
 439 static 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
 512 static 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
 535 static 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
 555 static 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
 564 static 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
 582 static 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
 640 static 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
 693 static 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
 767 static 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
 825 static 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
 854 static 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
 906 static 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
 938 static const struct of_device_id eqbr_pinctrl_dt_match[] = {
 939         { .compatible = "intel,lgm-io" },
 940         {}
 941 };
 942 MODULE_DEVICE_TABLE(of, eqbr_pinctrl_dt_match);
 943
 944 static 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
 952 module_platform_driver(eqbr_pinctrl_driver);
 953
 954 MODULE_AUTHOR("Zhu Yixin <yixin.zhu@intel.com>, Rahul Tanwar <rahul.tanwar@intel.com>");
 955 MODULE_DESCRIPTION("Pinctrl Driver for LGM SoC (Equilibrium)");
 956 MODULE_LICENSE("GPL v2");