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