Merge branch 'gpio/next' into next

* gpio/next: (12 commits)
  gpio : mpc8xxx : ls1088a/ls1028a edge detection mode bug fixs.
  gpio: mpc8xxx: Don't overwrite default irq_set_type callback
  gpio/mpc8xxx: change irq handler from chained to normal
  MLK-22733 gpio: mxc: use platform_get_irq_optional() to avoid error message
  gpio: pca953x: no need to do regcache sync without vcc regulator
  ...
This commit is contained in:
Dong Aisheng 2019-12-02 18:02:27 +08:00
commit a83b0af812
10 changed files with 774 additions and 28 deletions

View File

@ -0,0 +1,57 @@
Device-Tree bindings for drivers/gpio/gpio-imx-rpmsg.c gpio driver over
rpmsg. On i.mx7ULP PTA PTB are connected on M4 side, so rpmsg gpio driver
needed to get/set gpio status from M4 side by rpmsg.
Required properties:
- compatible : Should be "fsl,imx-rpmsg-gpio".
- port_idx : Specify the GPIO PORT index, PTA:0, PTB:1.
- gpio-controller : Mark the device node as a gpio controller.
- #gpio-cells : Should be two. The first cell is the pin number and
the second cell is used to specify the gpio polarity:
0 = active high
1 = active low
- interrupt-controller: Marks the device node as an interrupt controller.
- #interrupt-cells : Should be 2. The first cell is the GPIO number.
The second cell bits[3:0] is used to specify trigger type and level flags:
1 = low-to-high edge triggered.
2 = high-to-low edge triggered.
4 = active high level-sensitive.
8 = active low level-sensitive.
Note: Each GPIO port should have an alias correctly numbered in "aliases"
node.
Examples:
aliases {
gpio4 = &rpmsg_gpio0;
gpio5 = &rpmsg_gpio1;
};
rpmsg_gpio0: rpmsg-gpio0 {
compatible = "fsl,imx-rpmsg-gpio";
port_idx = <0>;
gpio-controller;
#gpio-cells = <2>;
#interrupt-cells = <2>;
interrupt-controller;
interrupt-parent = <&rpmsg_gpio0>;
status = "okay";
};
rpmsg_gpio1: rpmsg-gpio1 {
compatible = "fsl,imx-rpmsg-gpio";
port_idx = <1>;
gpio-controller;
#gpio-cells = <2>;
#interrupt-cells = <2>;
interrupt-controller;
interrupt-parent = <&rpmsg_gpio1>;
status = "okay";
};
&skeleton_node {
interrupt-parent = <&rpmsg_gpio1>;
interrupts = <7 2>;
wakeup-gpios = <&rpmsg_gpio1 7 GPIO_ACTIVE_LOW>;
};

View File

@ -31,6 +31,7 @@ Optional properties:
- first cell is the pin number
- second cell is used to specify flags
- interrupts: Interrupt specifier for the controllers interrupt.
- out-default: set the output IO default voltage. Exp: out-default = /bits/ 16 <mask val>;
Please refer to gpio.txt in this directory for details of the common GPIO
bindings used by client devices.

View File

@ -389,6 +389,13 @@ config GPIO_MXS
select GPIO_GENERIC
select GENERIC_IRQ_CHIP
config GPIO_MXC_PAD_WAKEUP
def_bool y
depends on IMX_SCU
select GPIO_MXC
help
Say Y here to enable the imx8 gpio pad wakeup
config GPIO_OCTEON
tristate "Cavium OCTEON GPIO"
depends on GPIOLIB && CAVIUM_OCTEON_SOC
@ -568,6 +575,18 @@ config GPIO_VF610
help
Say yes here to support Vybrid vf610 GPIOs.
config GPIO_IMX_RPMSG
bool "NXP i.MX7ULP RPMSG GPIO support"
depends on ARCH_MXC && RPMSG && GPIOLIB
help
This driver support i.MX7ULP RPMSG virtual GPIOs.
config GPIO_IMX_RPMSG
bool "NXP i.MX7ULP RPMSG GPIO support"
depends on ARCH_MXC && RPMSG && GPIOLIB
help
This driver support i.MX7ULP RPMSG virtual GPIOs.
config GPIO_VR41XX
tristate "NEC VR4100 series General-purpose I/O Uint support"
depends on CPU_VR41XX

View File

@ -151,6 +151,7 @@ obj-$(CONFIG_GPIO_TWL6040) += gpio-twl6040.o
obj-$(CONFIG_GPIO_UCB1400) += gpio-ucb1400.o
obj-$(CONFIG_GPIO_UNIPHIER) += gpio-uniphier.o
obj-$(CONFIG_GPIO_VF610) += gpio-vf610.o
obj-$(CONFIG_GPIO_IMX_RPMSG) += gpio-imx-rpmsg.o
obj-$(CONFIG_GPIO_VIPERBOARD) += gpio-viperboard.o
obj-$(CONFIG_GPIO_VR41XX) += gpio-vr41xx.o
obj-$(CONFIG_GPIO_VX855) += gpio-vx855.o

View File

@ -142,6 +142,9 @@ static int gen_74x164_probe(struct spi_device *spi)
chip->registers = nregs;
chip->gpio_chip.ngpio = GEN_74X164_NUMBER_GPIOS * chip->registers;
of_property_read_u8_array(spi->dev.of_node, "registers-default",
chip->buffer, chip->registers);
chip->gpio_chip.can_sleep = true;
chip->gpio_chip.parent = &spi->dev;
chip->gpio_chip.owner = THIS_MODULE;

View File

@ -0,0 +1,430 @@
/*
* Copyright 2017 NXP
*
* This program is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* as published by the Free Software Foundation; either version 2
* of the License, or (at your option) any later version.
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*/
#include <linux/kernel.h>
#include <linux/module.h>
#include <linux/bitops.h>
#include <linux/err.h>
#include <linux/gpio.h>
#include <linux/imx_rpmsg.h>
#include <linux/init.h>
#include <linux/irqdomain.h>
#include <linux/of.h>
#include <linux/of_device.h>
#include <linux/platform_device.h>
#include <linux/pm_qos.h>
#include <linux/rpmsg.h>
#include <linux/virtio.h>
#define IMX_RPMSG_GPIO_PER_PORT 32
#define RPMSG_TIMEOUT 1000
enum gpio_input_trigger_type {
GPIO_RPMSG_TRI_IGNORE,
GPIO_RPMSG_TRI_RISING,
GPIO_RPMSG_TRI_FALLING,
GPIO_RPMSG_TRI_BOTH_EDGE,
GPIO_RPMSG_TRI_LOW_LEVEL,
GPIO_RPMSG_TRI_HIGH_LEVEL,
};
enum gpio_rpmsg_header_type {
GPIO_RPMSG_SETUP,
GPIO_RPMSG_REPLY,
GPIO_RPMSG_NOTIFY,
};
enum gpio_rpmsg_header_cmd {
GPIO_RPMSG_INPUT_INIT,
GPIO_RPMSG_OUTPUT_INIT,
GPIO_RPMSG_INPUT_GET,
};
struct gpio_rpmsg_data {
struct imx_rpmsg_head header;
u8 pin_idx;
u8 port_idx;
union {
u8 event;
u8 retcode;
u8 value;
} out;
union {
u8 wakeup;
u8 value;
} in;
} __packed __aligned(8);
struct imx_rpmsg_gpio_port {
struct gpio_chip gc;
struct irq_chip chip;
struct irq_domain *domain;
struct gpio_rpmsg_data msg;
u32 irq_type[IMX_RPMSG_GPIO_PER_PORT];
int idx;
};
struct imx_gpio_rpmsg_info {
struct rpmsg_device *rpdev;
struct gpio_rpmsg_data *notify_msg;
struct gpio_rpmsg_data *reply_msg;
struct pm_qos_request pm_qos_req;
struct completion cmd_complete;
struct mutex lock;
};
static struct imx_gpio_rpmsg_info gpio_rpmsg;
static int gpio_send_message(struct imx_rpmsg_gpio_port *port,
struct gpio_rpmsg_data *msg,
struct imx_gpio_rpmsg_info *info,
bool sync)
{
int err;
if (!info->rpdev) {
dev_dbg(&info->rpdev->dev,
"rpmsg channel not ready, m4 image ready?\n");
return -EINVAL;
}
mutex_lock(&info->lock);
pm_qos_add_request(&info->pm_qos_req,
PM_QOS_CPU_DMA_LATENCY, 0);
reinit_completion(&info->cmd_complete);
err = rpmsg_send(info->rpdev->ept, (void *)msg,
sizeof(struct gpio_rpmsg_data));
if (err) {
dev_err(&info->rpdev->dev, "rpmsg_send failed: %d\n", err);
goto err_out;
}
if (sync) {
err = wait_for_completion_timeout(&info->cmd_complete,
msecs_to_jiffies(RPMSG_TIMEOUT));
if (!err) {
dev_err(&info->rpdev->dev, "rpmsg_send timeout!\n");
err = -ETIMEDOUT;
goto err_out;
}
if (info->reply_msg->out.retcode != 0) {
dev_err(&info->rpdev->dev, "rpmsg not ack %d!\n",
info->reply_msg->out.retcode);
err = -EINVAL;
goto err_out;
}
/* copy the reply message */
memcpy(&port->msg, info->reply_msg, sizeof(*info->reply_msg));
err = 0;
}
err_out:
pm_qos_remove_request(&info->pm_qos_req);
mutex_unlock(&info->lock);
return err;
}
static int gpio_rpmsg_cb(struct rpmsg_device *rpdev,
void *data, int len, void *priv, u32 src)
{
struct gpio_rpmsg_data *msg = (struct gpio_rpmsg_data *)data;
if (msg->header.type == GPIO_RPMSG_REPLY) {
gpio_rpmsg.reply_msg = msg;
complete(&gpio_rpmsg.cmd_complete);
} else if (msg->header.type == GPIO_RPMSG_NOTIFY) {
gpio_rpmsg.notify_msg = msg;
/* TBD for interrupt handler */
} else
dev_err(&gpio_rpmsg.rpdev->dev, "wrong command type!\n");
return 0;
}
static int imx_rpmsg_gpio_get(struct gpio_chip *gc, unsigned int gpio)
{
struct imx_rpmsg_gpio_port *port = gpiochip_get_data(gc);
struct gpio_rpmsg_data msg;
int ret;
memset(&msg, 0, sizeof(struct gpio_rpmsg_data));
msg.header.cate = IMX_RPMSG_GPIO;
msg.header.major = IMX_RMPSG_MAJOR;
msg.header.minor = IMX_RMPSG_MINOR;
msg.header.type = GPIO_RPMSG_SETUP;
msg.header.cmd = GPIO_RPMSG_INPUT_GET;
msg.pin_idx = gpio;
msg.port_idx = port->idx;
ret = gpio_send_message(port, &msg, &gpio_rpmsg, true);
if (!ret)
return !!port->msg.in.value;
return ret;
}
static int imx_rpmsg_gpio_direction_input(struct gpio_chip *gc,
unsigned int gpio)
{
struct imx_rpmsg_gpio_port *port = gpiochip_get_data(gc);
struct gpio_rpmsg_data msg;
memset(&msg, 0, sizeof(struct gpio_rpmsg_data));
msg.header.cate = IMX_RPMSG_GPIO;
msg.header.major = IMX_RMPSG_MAJOR;
msg.header.minor = IMX_RMPSG_MINOR;
msg.header.type = GPIO_RPMSG_SETUP;
msg.header.cmd = GPIO_RPMSG_INPUT_INIT;
msg.pin_idx = gpio;
msg.port_idx = port->idx;
/* TBD: get event trigger and wakeup from GPIO descriptor */
msg.out.event = GPIO_RPMSG_TRI_IGNORE;
msg.in.wakeup = 0;
return gpio_send_message(port, &msg, &gpio_rpmsg, true);
}
static inline void imx_rpmsg_gpio_direction_output_init(struct gpio_chip *gc,
unsigned int gpio, int val, struct gpio_rpmsg_data *msg)
{
struct imx_rpmsg_gpio_port *port = gpiochip_get_data(gc);
msg->header.cate = IMX_RPMSG_GPIO;
msg->header.major = IMX_RMPSG_MAJOR;
msg->header.minor = IMX_RMPSG_MINOR;
msg->header.type = GPIO_RPMSG_SETUP;
msg->header.cmd = GPIO_RPMSG_OUTPUT_INIT;
msg->pin_idx = gpio;
msg->port_idx = port->idx;
msg->out.value = val;
}
static void imx_rpmsg_gpio_set(struct gpio_chip *gc, unsigned int gpio, int val)
{
struct imx_rpmsg_gpio_port *port = gpiochip_get_data(gc);
struct gpio_rpmsg_data msg;
memset(&msg, 0, sizeof(struct gpio_rpmsg_data));
imx_rpmsg_gpio_direction_output_init(gc, gpio, val, &msg);
gpio_send_message(port, &msg, &gpio_rpmsg, true);
}
static int imx_rpmsg_gpio_direction_output(struct gpio_chip *gc,
unsigned int gpio, int val)
{
struct imx_rpmsg_gpio_port *port = gpiochip_get_data(gc);
struct gpio_rpmsg_data msg;
memset(&msg, 0, sizeof(struct gpio_rpmsg_data));
imx_rpmsg_gpio_direction_output_init(gc, gpio, val, &msg);
return gpio_send_message(port, &msg, &gpio_rpmsg, true);
}
static int gpio_rpmsg_probe(struct rpmsg_device *rpdev)
{
gpio_rpmsg.rpdev = rpdev;
dev_info(&rpdev->dev, "new channel: 0x%x -> 0x%x!\n",
rpdev->src, rpdev->dst);
init_completion(&gpio_rpmsg.cmd_complete);
mutex_init(&gpio_rpmsg.lock);
return 0;
}
static struct rpmsg_device_id gpio_rpmsg_id_table[] = {
{ .name = "rpmsg-io-channel" },
{},
};
static struct rpmsg_driver gpio_rpmsg_driver = {
.drv.name = "gpio_rpmsg",
.drv.owner = THIS_MODULE,
.id_table = gpio_rpmsg_id_table,
.probe = gpio_rpmsg_probe,
.callback = gpio_rpmsg_cb,
};
static int imx_rpmsg_irq_set_type(struct irq_data *d, u32 type)
{
struct imx_rpmsg_gpio_port *port = irq_data_get_irq_chip_data(d);
u32 gpio_idx = d->hwirq;
int edge = 0;
int ret = 0;
switch (type) {
case IRQ_TYPE_EDGE_RISING:
edge = GPIO_RPMSG_TRI_RISING;
break;
case IRQ_TYPE_EDGE_FALLING:
edge = GPIO_RPMSG_TRI_FALLING;
break;
case IRQ_TYPE_EDGE_BOTH:
edge = GPIO_RPMSG_TRI_BOTH_EDGE;
break;
case IRQ_TYPE_LEVEL_LOW:
edge = GPIO_RPMSG_TRI_LOW_LEVEL;
break;
case IRQ_TYPE_LEVEL_HIGH:
edge = GPIO_RPMSG_TRI_HIGH_LEVEL;
break;
default:
ret = -EINVAL;
break;
}
port->irq_type[gpio_idx] = edge;
return ret;
}
static int imx_rpmsg_irq_set_wake(struct irq_data *d, u32 enable)
{
struct imx_rpmsg_gpio_port *port = irq_data_get_irq_chip_data(d);
struct gpio_rpmsg_data msg;
u32 gpio_idx = d->hwirq;
memset(&msg, 0, sizeof(struct gpio_rpmsg_data));
msg.header.cate = IMX_RPMSG_GPIO;
msg.header.major = IMX_RMPSG_MAJOR;
msg.header.minor = IMX_RMPSG_MINOR;
msg.header.type = GPIO_RPMSG_SETUP;
msg.header.cmd = GPIO_RPMSG_INPUT_INIT;
msg.pin_idx = gpio_idx;
msg.port_idx = port->idx;
/* set wakeup trigger source,
* if not set irq type, then use high level as trigger type
*/
msg.out.event = port->irq_type[gpio_idx];
if (!msg.out.event)
msg.out.event = GPIO_RPMSG_TRI_HIGH_LEVEL;
msg.in.wakeup = enable;
/* here should be atomic context */
gpio_send_message(port, &msg, &gpio_rpmsg, false);
return 0;
}
static void imx_rpmsg_unmask_irq(struct irq_data *d)
{
/* No need to implement the callback */
}
static void imx_rpmsg_mask_irq(struct irq_data *d)
{
/* No need to implement the callback */
}
static struct irq_chip imx_rpmsg_irq_chip = {
.irq_mask = imx_rpmsg_mask_irq,
.irq_unmask = imx_rpmsg_unmask_irq,
.irq_set_wake = imx_rpmsg_irq_set_wake,
.irq_set_type = imx_rpmsg_irq_set_type,
};
static int imx_rpmsg_gpio_probe(struct platform_device *pdev)
{
struct device *dev = &pdev->dev;
struct device_node *np = dev->of_node;
struct imx_rpmsg_gpio_port *port;
struct gpio_chip *gc;
int i, irq_base;
int ret;
port = devm_kzalloc(&pdev->dev, sizeof(*port), GFP_KERNEL);
if (!port)
return -ENOMEM;
ret = of_property_read_u32(np, "port_idx", &port->idx);
if (ret)
return ret;
gc = &port->gc;
gc->of_node = np;
gc->parent = dev;
gc->label = "imx-rpmsg-gpio";
gc->ngpio = IMX_RPMSG_GPIO_PER_PORT;
gc->base = of_alias_get_id(np, "gpio") * IMX_RPMSG_GPIO_PER_PORT;
gc->direction_input = imx_rpmsg_gpio_direction_input;
gc->direction_output = imx_rpmsg_gpio_direction_output;
gc->get = imx_rpmsg_gpio_get;
gc->set = imx_rpmsg_gpio_set;
platform_set_drvdata(pdev, port);
ret = devm_gpiochip_add_data(dev, gc, port);
if (ret < 0)
return ret;
/* generate one new irq domain */
port->chip = imx_rpmsg_irq_chip;
port->chip.name = kasprintf(GFP_KERNEL, "rpmsg-irq-port-%d", port->idx);
port->chip.parent_device = NULL;
irq_base = irq_alloc_descs(-1, 0, IMX_RPMSG_GPIO_PER_PORT,
numa_node_id());
WARN_ON(irq_base < 0);
port->domain = irq_domain_add_legacy(np, IMX_RPMSG_GPIO_PER_PORT,
irq_base, 0,
&irq_domain_simple_ops, port);
WARN_ON(!port->domain);
for (i = irq_base; i < irq_base + IMX_RPMSG_GPIO_PER_PORT; i++) {
irq_set_chip_and_handler(i, &port->chip, handle_level_irq);
irq_set_chip_data(i, port);
irq_clear_status_flags(i, IRQ_NOREQUEST);
irq_set_probe(i);
}
return 0;
}
static const struct of_device_id imx_rpmsg_gpio_dt_ids[] = {
{ .compatible = "fsl,imx-rpmsg-gpio" },
{ /* sentinel */ }
};
static struct platform_driver imx_rpmsg_gpio_driver = {
.driver = {
.name = "gpio-imx-rpmsg",
.of_match_table = imx_rpmsg_gpio_dt_ids,
},
.probe = imx_rpmsg_gpio_probe,
};
static int __init gpio_imx_rpmsg_init(void)
{
int ret;
ret = register_rpmsg_driver(&gpio_rpmsg_driver);
if (ret)
return ret;
return platform_driver_register(&imx_rpmsg_gpio_driver);
}
device_initcall(gpio_imx_rpmsg_init);
MODULE_AUTHOR("NXP Semiconductor");
MODULE_DESCRIPTION("NXP i.MX7ULP rpmsg gpio driver");
MODULE_LICENSE("GPL v2");

View File

@ -19,6 +19,7 @@
#include <linux/i2c.h>
#include <linux/platform_data/max732x.h>
#include <linux/of.h>
#include <linux/reset.h>
/*
@ -76,6 +77,12 @@
#define INT_CAPS(x) (((uint64_t)(x)) << 32)
enum {
OUTPUT_MASK,
OUTPUT_VAL,
OUTPUT_NUM,
};
enum {
MAX7319,
MAX7320,
@ -622,6 +629,8 @@ static int max732x_probe(struct i2c_client *client,
struct i2c_client *c;
uint16_t addr_a, addr_b;
int ret, nr_port;
u16 out_set[OUTPUT_NUM];
unsigned long mask, val;
pdata = dev_get_platdata(&client->dev);
node = client->dev.of_node;
@ -639,6 +648,10 @@ static int max732x_probe(struct i2c_client *client,
return -ENOMEM;
chip->client = client;
ret = device_reset(&client->dev);
if (ret == -EPROBE_DEFER)
return ret;
nr_port = max732x_setup_gpio(chip, id, pdata->gpio_base);
chip->gpio_chip.parent = &client->dev;
@ -711,6 +724,15 @@ static int max732x_probe(struct i2c_client *client,
}
i2c_set_clientdata(client, chip);
/* set the output IO default voltage */
if (!of_property_read_u16_array(node, "out-default", out_set,
ARRAY_SIZE(out_set))) {
mask = out_set[OUTPUT_MASK] & chip->dir_output;
val = out_set[OUTPUT_VAL];
max732x_gpio_set_multiple(&chip->gpio_chip, &mask, &val);
}
return 0;
}

View File

@ -22,6 +22,7 @@
#include <linux/irq.h>
#include <linux/gpio/driver.h>
#include <linux/bitops.h>
#include <linux/interrupt.h>
#define MPC8XXX_GPIO_PINS 32
@ -127,20 +128,19 @@ static int mpc8xxx_gpio_to_irq(struct gpio_chip *gc, unsigned offset)
return -ENXIO;
}
static void mpc8xxx_gpio_irq_cascade(struct irq_desc *desc)
static irqreturn_t mpc8xxx_gpio_irq_cascade(int irq, void *data)
{
struct mpc8xxx_gpio_chip *mpc8xxx_gc = irq_desc_get_handler_data(desc);
struct irq_chip *chip = irq_desc_get_chip(desc);
struct mpc8xxx_gpio_chip *mpc8xxx_gc = data;
struct gpio_chip *gc = &mpc8xxx_gc->gc;
unsigned int mask;
unsigned long mask;
int i;
mask = gc->read_reg(mpc8xxx_gc->regs + GPIO_IER)
& gc->read_reg(mpc8xxx_gc->regs + GPIO_IMR);
if (mask)
generic_handle_irq(irq_linear_revmap(mpc8xxx_gc->irq,
32 - ffs(mask)));
if (chip->irq_eoi)
chip->irq_eoi(&desc->irq_data);
for_each_set_bit(i, &mask, 32)
generic_handle_irq(irq_linear_revmap(mpc8xxx_gc->irq, 31 - i));
return IRQ_HANDLED;
}
static void mpc8xxx_irq_unmask(struct irq_data *d)
@ -296,6 +296,7 @@ static const struct mpc8xxx_gpio_devtype mpc512x_gpio_devtype = {
static const struct mpc8xxx_gpio_devtype ls1028a_gpio_devtype = {
.gpio_dir_in_init = ls1028a_gpio_dir_in_init,
.irq_set_type = mpc8xxx_irq_set_type,
};
static const struct mpc8xxx_gpio_devtype mpc5125_gpio_devtype = {
@ -377,7 +378,8 @@ static int mpc8xxx_probe(struct platform_device *pdev)
* It's assumed that only a single type of gpio controller is available
* on the current machine, so overwriting global data is fine.
*/
mpc8xxx_irq_chip.irq_set_type = devtype->irq_set_type;
if (devtype->irq_set_type)
mpc8xxx_irq_chip.irq_set_type = devtype->irq_set_type;
if (devtype->gpio_dir_out)
gc->direction_output = devtype->gpio_dir_out;
@ -409,8 +411,16 @@ static int mpc8xxx_probe(struct platform_device *pdev)
if (devtype->gpio_dir_in_init)
devtype->gpio_dir_in_init(gc);
irq_set_chained_handler_and_data(mpc8xxx_gc->irqn,
mpc8xxx_gpio_irq_cascade, mpc8xxx_gc);
ret = devm_request_irq(&pdev->dev, mpc8xxx_gc->irqn,
mpc8xxx_gpio_irq_cascade,
IRQF_NO_THREAD | IRQF_SHARED, "gpio-cascade",
mpc8xxx_gc);
if (ret) {
dev_err(&pdev->dev, "%s: failed to devm_request_irq(%d), ret = %d\n",
np->full_name, mpc8xxx_gc->irqn, ret);
goto err;
}
return 0;
err:
iounmap(mpc8xxx_gc->regs);

View File

@ -22,6 +22,15 @@
#include <linux/of.h>
#include <linux/of_device.h>
#include <linux/bug.h>
#ifdef CONFIG_GPIO_MXC_PAD_WAKEUP
#include <linux/firmware/imx/sci.h>
#define IMX_SC_PAD_FUNC_GET_WAKEUP 9
#define IMX_SC_PAD_FUNC_SET_WAKEUP 4
#define IMX_SC_PAD_WAKEUP_OFF 0
#define IMX_SC_IRQ_GROUP_WAKE 3
#define IMX_SC_IRQ_PAD (1 << 1)
#endif
enum mxc_gpio_hwtype {
IMX1_GPIO, /* runs on i.mx1 */
@ -30,6 +39,33 @@ enum mxc_gpio_hwtype {
IMX35_GPIO, /* runs on all other i.mx */
};
#ifdef CONFIG_GPIO_MXC_PAD_WAKEUP
struct mxc_gpio_pad_wakeup {
u32 pin_id;
u32 type;
u32 line;
};
struct imx_sc_msg_gpio_get_pad_wakeup {
struct imx_sc_rpc_msg hdr;
union {
struct req_pad {
u16 pad;
} __packed req;
struct resp_wakeup {
u8 wakeup;
} resp;
} data;
} __packed;
struct imx_sc_msg_gpio_set_pad_wakeup {
struct imx_sc_rpc_msg hdr;
u16 pad;
u8 wakeup;
} __packed;
#endif
/* device type dependent stuff */
struct mxc_gpio_hwdata {
unsigned dr_reg;
@ -67,8 +103,16 @@ struct mxc_gpio_port {
u32 both_edges;
struct mxc_gpio_reg_saved gpio_saved_reg;
bool power_off;
#ifdef CONFIG_GPIO_MXC_PAD_WAKEUP
u32 pad_wakeup_num;
struct mxc_gpio_pad_wakeup pad_wakeup[32];
#endif
};
#ifdef CONFIG_GPIO_MXC_PAD_WAKEUP
static struct imx_sc_ipc *gpio_ipc_handle;
#endif
static struct mxc_gpio_hwdata imx1_imx21_gpio_hwdata = {
.dr_reg = 0x1c,
.gdir_reg = 0x00,
@ -310,6 +354,85 @@ static void mx2_gpio_irq_handler(struct irq_desc *desc)
chained_irq_exit(chip, desc);
}
#ifdef CONFIG_GPIO_MXC_PAD_WAKEUP
static int mxc_gpio_get_pad_wakeup(struct mxc_gpio_port *port)
{
struct imx_sc_msg_gpio_get_pad_wakeup msg;
struct imx_sc_rpc_msg *hdr = &msg.hdr;
u8 wakeup_type;
int ret;
int i;
hdr->ver = IMX_SC_RPC_VERSION;
hdr->svc = IMX_SC_RPC_SVC_PAD;
hdr->func = IMX_SC_PAD_FUNC_GET_WAKEUP;
hdr->size = 2;
for (i = 0; i < port->pad_wakeup_num; i++) {
/* get original pad type */
wakeup_type = port->pad_wakeup[i].type;
msg.data.req.pad = port->pad_wakeup[i].pin_id;
ret = imx_scu_call_rpc(gpio_ipc_handle, &msg, true);
if (ret) {
dev_err(port->gc.parent, "get pad wakeup failed, ret %d\n", ret);
return ret;
}
wakeup_type = msg.data.resp.wakeup;
/* return wakeup gpio pin's line */
if (wakeup_type != port->pad_wakeup[i].type)
return port->pad_wakeup[i].line;
}
return -EINVAL;
}
static void mxc_gpio_set_pad_wakeup(struct mxc_gpio_port *port, bool enable)
{
struct imx_sc_msg_gpio_set_pad_wakeup msg;
struct imx_sc_rpc_msg *hdr = &msg.hdr;
int ret;
int i;
hdr->ver = IMX_SC_RPC_VERSION;
hdr->svc = IMX_SC_RPC_SVC_PAD;
hdr->func = IMX_SC_PAD_FUNC_SET_WAKEUP;
hdr->size = 2;
for (i = 0; i < port->pad_wakeup_num; i++) {
msg.pad = port->pad_wakeup[i].pin_id;
msg.wakeup = enable ? port->pad_wakeup[i].type : IMX_SC_PAD_WAKEUP_OFF;
ret = imx_scu_call_rpc(gpio_ipc_handle, &msg, true);
if (ret) {
dev_err(port->gc.parent, "set pad wakeup failed, ret %d\n", ret);
return;
}
}
}
static void mxc_gpio_handle_pad_wakeup(struct mxc_gpio_port *port, int line)
{
struct irq_desc *desc = irq_to_desc(port->irq);
struct irq_chip *chip = irq_desc_get_chip(desc);
u32 irq_stat;
/* skip invalid line */
if (line > 31) {
dev_err(port->gc.parent, "invalid wakeup line %d\n", line);
return;
}
dev_info(port->gc.parent, "wakeup by pad, line %d\n", line);
chained_irq_enter(chip, desc);
irq_stat = (1 << line);
mxc_gpio_irq_handler(port, irq_stat);
chained_irq_exit(chip, desc);
}
#endif
/*
* Set interrupt number "irq" in the GPIO as a wake-up source.
* While system is running, all registered GPIO interrupts need to have
@ -413,6 +536,9 @@ static int mxc_gpio_probe(struct platform_device *pdev)
struct mxc_gpio_port *port;
int irq_base;
int err;
#ifdef CONFIG_GPIO_MXC_PAD_WAKEUP
int i;
#endif
mxc_gpio_get_hw(pdev);
@ -426,7 +552,7 @@ static int mxc_gpio_probe(struct platform_device *pdev)
if (IS_ERR(port->base))
return PTR_ERR(port->base);
port->irq_high = platform_get_irq(pdev, 1);
port->irq_high = platform_get_irq_optional(pdev, 1);
if (port->irq_high < 0)
port->irq_high = 0;
@ -445,6 +571,35 @@ static int mxc_gpio_probe(struct platform_device *pdev)
return err;
}
#ifdef CONFIG_GPIO_MXC_PAD_WAKEUP
/*
* parse pad wakeup info from dtb, each pad has to provide
* <pin_id, type, line>, these info should be put in each
* gpio node and with a "pad-wakeup-num" to indicate the
* total lines are with pad wakeup enabled.
*/
if (!of_property_read_u32(np, "pad-wakeup-num", &port->pad_wakeup_num)) {
if (port->pad_wakeup_num != 0) {
if (!gpio_ipc_handle) {
err = imx_scu_get_handle(&gpio_ipc_handle);
if (err)
return err;
}
for (i = 0; i < port->pad_wakeup_num; i++) {
of_property_read_u32_index(np, "pad-wakeup",
i * 3 + 0, &port->pad_wakeup[i].pin_id);
of_property_read_u32_index(np, "pad-wakeup",
i * 3 + 1, &port->pad_wakeup[i].type);
of_property_read_u32_index(np, "pad-wakeup",
i * 3 + 2, &port->pad_wakeup[i].line);
}
err = imx_scu_irq_group_enable(IMX_SC_IRQ_GROUP_WAKE, IMX_SC_IRQ_PAD, true);
if (err)
dev_warn(&pdev->dev, "Enable irq failed, GPIO pad wakeup NOT supported\n");
}
}
#endif
if (of_device_is_compatible(np, "fsl,imx7d-gpio"))
port->power_off = true;
@ -549,6 +704,36 @@ static void mxc_gpio_restore_regs(struct mxc_gpio_port *port)
writel(port->gpio_saved_reg.dr, port->base + GPIO_DR);
}
static int __maybe_unused mxc_gpio_noirq_suspend(struct device *dev)
{
#ifdef CONFIG_GPIO_MXC_PAD_WAKEUP
struct platform_device *pdev = to_platform_device(dev);
struct mxc_gpio_port *port = platform_get_drvdata(pdev);
mxc_gpio_set_pad_wakeup(port, true);
#endif
return 0;
}
static int __maybe_unused mxc_gpio_noirq_resume(struct device *dev)
{
#ifdef CONFIG_GPIO_MXC_PAD_WAKEUP
struct platform_device *pdev = to_platform_device(dev);
struct mxc_gpio_port *port = platform_get_drvdata(pdev);
int wakeup_line = mxc_gpio_get_pad_wakeup(port);
mxc_gpio_set_pad_wakeup(port, false);
if (wakeup_line >= 0)
mxc_gpio_handle_pad_wakeup(port, wakeup_line);
#endif
return 0;
}
static const struct dev_pm_ops mxc_gpio_dev_pm_ops = {
SET_NOIRQ_SYSTEM_SLEEP_PM_OPS(mxc_gpio_noirq_suspend, mxc_gpio_noirq_resume)
};
static int mxc_gpio_syscore_suspend(void)
{
struct mxc_gpio_port *port;
@ -588,6 +773,7 @@ static struct platform_driver mxc_gpio_driver = {
.name = "gpio-mxc",
.of_match_table = mxc_gpio_dt_ids,
.suppress_bind_attrs = true,
.pm = &mxc_gpio_dev_pm_ops,
},
.probe = mxc_gpio_probe,
.id_table = mxc_gpio_devtype,

View File

@ -20,6 +20,7 @@
#include <linux/platform_data/pca953x.h>
#include <linux/regmap.h>
#include <linux/regulator/consumer.h>
#include <linux/reset.h>
#include <linux/slab.h>
#include <asm/unaligned.h>
@ -930,17 +931,21 @@ static int pca953x_probe(struct i2c_client *client,
chip->client = client;
reg = devm_regulator_get(&client->dev, "vcc");
reg = devm_regulator_get_optional(&client->dev, "vcc");
if (IS_ERR(reg)) {
ret = PTR_ERR(reg);
if (ret != -EPROBE_DEFER)
dev_err(&client->dev, "reg get err: %d\n", ret);
return ret;
if (ret == -EPROBE_DEFER)
return ret;
dev_err(&client->dev, "reg get err: %d\n", ret);
reg = NULL;
}
ret = regulator_enable(reg);
if (ret) {
dev_err(&client->dev, "reg en err: %d\n", ret);
return ret;
if (reg) {
ret = regulator_enable(reg);
if (ret) {
dev_err(&client->dev, "reg en err: %d\n", ret);
return ret;
}
}
chip->regulator = reg;
@ -988,6 +993,10 @@ static int pca953x_probe(struct i2c_client *client,
lockdep_set_subclass(&chip->i2c_lock,
i2c_adapter_depth(client->adapter));
ret = device_reset(&client->dev);
if (ret == -EPROBE_DEFER)
return -EPROBE_DEFER;
/* initialize cached registers from their original values.
* we can't share this chip with another i2c master.
*/
@ -1021,7 +1030,8 @@ static int pca953x_probe(struct i2c_client *client,
return 0;
err_exit:
regulator_disable(chip->regulator);
if (chip->regulator)
regulator_disable(chip->regulator);
return ret;
}
@ -1040,7 +1050,8 @@ static int pca953x_remove(struct i2c_client *client)
ret = 0;
}
regulator_disable(chip->regulator);
if (chip->regulator)
regulator_disable(chip->regulator);
return ret;
}
@ -1101,7 +1112,8 @@ static int pca953x_suspend(struct device *dev)
if (atomic_read(&chip->wakeup_path))
device_set_wakeup_path(dev);
else
regulator_disable(chip->regulator);
if (chip->regulator)
regulator_disable(chip->regulator);
return 0;
}
@ -1111,15 +1123,20 @@ static int pca953x_resume(struct device *dev)
struct pca953x_chip *chip = dev_get_drvdata(dev);
int ret;
regcache_cache_only(chip->regmap, false);
if (!atomic_read(&chip->wakeup_path)) {
ret = regulator_enable(chip->regulator);
if (ret) {
dev_err(dev, "Failed to enable regulator: %d\n", ret);
if (chip->regulator) {
ret = regulator_enable(chip->regulator);
if (ret) {
dev_err(dev, "Failed to enable regulator: %d\n", ret);
return 0;
}
} else {
return 0;
}
}
regcache_cache_only(chip->regmap, false);
regcache_mark_dirty(chip->regmap);
ret = pca953x_regcache_sync(dev);
if (ret)