fix: drivers patch make raspberry pi 5 work properly

1 files changed, 119 insertions, 5 deletions

diff --git a/drivers/gpio/gpio-mmio.c b/drivers/gpio/gpio-mmio.c
index d89e78f0ead3..36bb04afa563 100644
--- a/drivers/gpio/gpio-mmio.c
+++ b/drivers/gpio/gpio-mmio.c
@@ -231,6 +231,25 @@ static void bgpio_set(struct gpio_chip *gc, unsigned int gpio, int val)
 	raw_spin_unlock_irqrestore(&gc->bgpio_lock, flags);
 }
 
+static void bgpio_set_direct(struct gpio_chip *gc, unsigned int gpio, int val)
+{
+	unsigned long mask = bgpio_line2mask(gc, gpio);
+	unsigned long flags;
+
+	raw_spin_lock_irqsave(&gc->bgpio_lock, flags);
+
+	gc->bgpio_data = gc->read_reg(gc->reg_dat);
+
+	if (val)
+		gc->bgpio_data |= mask;
+	else
+		gc->bgpio_data &= ~mask;
+
+	gc->write_reg(gc->reg_dat, gc->bgpio_data);
+
+	raw_spin_unlock_irqrestore(&gc->bgpio_lock, flags);
+}
+
 static void bgpio_set_with_clear(struct gpio_chip *gc, unsigned int gpio,
 				 int val)
 {
@@ -323,6 +342,27 @@ static void bgpio_set_multiple_with_clear(struct gpio_chip *gc,
 		gc->write_reg(gc->reg_clr, clear_mask);
 }
 
+static void bgpio_set_multiple_direct(struct gpio_chip *gc,
+				      unsigned long *mask,
+				      unsigned long *bits)
+{
+	unsigned long flags;
+	unsigned long set_mask, clear_mask;
+
+	raw_spin_lock_irqsave(&gc->bgpio_lock, flags);
+
+	bgpio_multiple_get_masks(gc, mask, bits, &set_mask, &clear_mask);
+
+	gc->bgpio_data = gc->read_reg(gc->reg_dat);
+
+	gc->bgpio_data |= set_mask;
+	gc->bgpio_data &= ~clear_mask;
+
+	gc->write_reg(gc->reg_dat, gc->bgpio_data);
+
+	raw_spin_unlock_irqrestore(&gc->bgpio_lock, flags);
+}
+
 static int bgpio_simple_dir_in(struct gpio_chip *gc, unsigned int gpio)
 {
 	return 0;
@@ -360,6 +400,29 @@ static int bgpio_dir_in(struct gpio_chip *gc, unsigned int gpio)
 	return 0;
 }
 
+static int bgpio_dir_in_direct(struct gpio_chip *gc, unsigned int gpio)
+{
+	unsigned long flags;
+
+	raw_spin_lock_irqsave(&gc->bgpio_lock, flags);
+
+	if (gc->reg_dir_in)
+		gc->bgpio_dir = ~gc->read_reg(gc->reg_dir_in);
+	if (gc->reg_dir_out)
+		gc->bgpio_dir = gc->read_reg(gc->reg_dir_out);
+
+	gc->bgpio_dir &= ~bgpio_line2mask(gc, gpio);
+
+	if (gc->reg_dir_in)
+		gc->write_reg(gc->reg_dir_in, ~gc->bgpio_dir);
+	if (gc->reg_dir_out)
+		gc->write_reg(gc->reg_dir_out, gc->bgpio_dir);
+
+	raw_spin_unlock_irqrestore(&gc->bgpio_lock, flags);
+
+	return 0;
+}
+
 static int bgpio_get_dir(struct gpio_chip *gc, unsigned int gpio)
 {
 	/* Return 0 if output, 1 if input */
@@ -398,6 +461,28 @@ static void bgpio_dir_out(struct gpio_chip *gc, unsigned int gpio, int val)
 	raw_spin_unlock_irqrestore(&gc->bgpio_lock, flags);
 }
 
+static void bgpio_dir_out_direct(struct gpio_chip *gc, unsigned int gpio,
+				 int val)
+{
+	unsigned long flags;
+
+	raw_spin_lock_irqsave(&gc->bgpio_lock, flags);
+
+	if (gc->reg_dir_in)
+		gc->bgpio_dir = ~gc->read_reg(gc->reg_dir_in);
+	if (gc->reg_dir_out)
+		gc->bgpio_dir = gc->read_reg(gc->reg_dir_out);
+
+	gc->bgpio_dir |= bgpio_line2mask(gc, gpio);
+
+	if (gc->reg_dir_in)
+		gc->write_reg(gc->reg_dir_in, ~gc->bgpio_dir);
+	if (gc->reg_dir_out)
+		gc->write_reg(gc->reg_dir_out, gc->bgpio_dir);
+
+	raw_spin_unlock_irqrestore(&gc->bgpio_lock, flags);
+}
+
 static int bgpio_dir_out_dir_first(struct gpio_chip *gc, unsigned int gpio,
 				   int val)
 {
@@ -414,6 +499,22 @@ static int bgpio_dir_out_val_first(struct gpio_chip *gc, unsigned int gpio,
 	return 0;
 }
 
+static int bgpio_dir_out_dir_first_direct(struct gpio_chip *gc,
+					  unsigned int gpio, int val)
+{
+	bgpio_dir_out_direct(gc, gpio, val);
+	gc->set(gc, gpio, val);
+	return 0;
+}
+
+static int bgpio_dir_out_val_first_direct(struct gpio_chip *gc,
+					  unsigned int gpio, int val)
+{
+	gc->set(gc, gpio, val);
+	bgpio_dir_out_direct(gc, gpio, val);
+	return 0;
+}
+
 static int bgpio_setup_accessors(struct device *dev,
 				 struct gpio_chip *gc,
 				 bool byte_be)
@@ -507,6 +608,9 @@ static int bgpio_setup_io(struct gpio_chip *gc,
 	} else if (flags & BGPIOF_NO_OUTPUT) {
 		gc->set = bgpio_set_none;
 		gc->set_multiple = NULL;
+	} else if (flags & BGPIOF_REG_DIRECT) {
+		gc->set = bgpio_set_direct;
+		gc->set_multiple = bgpio_set_multiple_direct;
 	} else {
 		gc->set = bgpio_set;
 		gc->set_multiple = bgpio_set_multiple;
@@ -543,11 +647,21 @@ static int bgpio_setup_direction(struct gpio_chip *gc,
 	if (dirout || dirin) {
 		gc->reg_dir_out = dirout;
 		gc->reg_dir_in = dirin;
-		if (flags & BGPIOF_NO_SET_ON_INPUT)
-			gc->direction_output = bgpio_dir_out_dir_first;
-		else
-			gc->direction_output = bgpio_dir_out_val_first;
-		gc->direction_input = bgpio_dir_in;
+		if (flags & BGPIOF_REG_DIRECT) {
+			if (flags & BGPIOF_NO_SET_ON_INPUT)
+				gc->direction_output =
+					bgpio_dir_out_dir_first_direct;
+			else
+				gc->direction_output =
+					bgpio_dir_out_val_first_direct;
+			gc->direction_input = bgpio_dir_in_direct;
+		} else {
+			if (flags & BGPIOF_NO_SET_ON_INPUT)
+				gc->direction_output = bgpio_dir_out_dir_first;
+			else
+				gc->direction_output = bgpio_dir_out_val_first;
+			gc->direction_input = bgpio_dir_in;
+		}
 		gc->get_direction = bgpio_get_dir;
 	} else {
 		if (flags & BGPIOF_NO_OUTPUT)