From 4a629ee5cb8135ec1bcde95d59eb11f61a698a61 Mon Sep 17 00:00:00 2001
From: Steve Jeong <steve@how2flow.net>
Date: Thu, 9 May 2024 03:05:58 +0000
Subject: [PATCH] ODROID-M1/S: Fix PWM mux error

Signed-off-by: Steve Jeong <steve@how2flow.net>
Change-Id: I17b12bf8762a5beb1ab149074ab27eaac4234967
---
 wiringPi/odroidm1.c  | 11 ++++++++---
 wiringPi/odroidm1.h  |  1 +
 wiringPi/odroidm1s.c | 11 ++++++++---
 3 files changed, 17 insertions(+), 6 deletions(-)

diff --git a/wiringPi/odroidm1.c b/wiringPi/odroidm1.c
index b095983..16998f3 100644
--- a/wiringPi/odroidm1.c
+++ b/wiringPi/odroidm1.c
@@ -425,13 +425,18 @@ static int setIomuxMode (int pin, int mode)
 	bank = (bank != 0);
 	data = *(grf[bank] + regOffset);
 
-	// Common IOMUX Funtion 1 : GPIO (3'h0)
 	switch (mode) {
 	case M1_FUNC_GPIO: // Common IOMUX Function 1_GPIO (3'h0)
 		data &= ~(0x7 << ((groupOffset % 4) * 4)); // ~0x07 = 3'h0
 		data |= (0x7 << ((groupOffset % 4) * 4 + 16)); // write_mask
 		*(grf[bank] + regOffset) = data;
 		break;
+	case M1_FUNC_PWM: // gpio0_B5/B6: 3'h100 gpio3_B2: 3'h101
+		data |= (bank == 0 ? (0x4 << ((groupOffset % 4) * 4)) : (0x5 << ((groupOffset % 4) * 4)));
+		data &= (bank == 0 ? ~(0x3 << ((groupOffset % 4) * 4)) : ~(0x2 << ((groupOffset % 4) * 4)));
+		data |= (0x7 << ((groupOffset % 4) * 4 + 16)); // write_mask
+		*(grf[bank] + regOffset) = data;
+		break;
 	default:
 		break;
 	}
@@ -460,9 +465,7 @@ static int _pinMode (int pin, int mode)
 	pwmRelease(origPin);
 	softPwmStop(origPin);
 	softToneStop(origPin);
-
 	setClkState(bank, M1_CLK_ENABLE);
-	setIomuxMode(origPin, M1_FUNC_GPIO);
 
 	data = *(gpio[bank] + regOffset);
 
@@ -473,6 +476,7 @@ static int _pinMode (int pin, int mode)
 			_pullUpDnControl(origPin, mode);
 			__attribute__((fallthrough));
 		case OUTPUT:
+			setIomuxMode(origPin, M1_FUNC_GPIO);
 			mode = (mode == OUTPUT);
 			data &= ~(1 << gpioToShiftRegBy16(pin));
 			data |=(mode << gpioToShiftRegBy16(pin));
@@ -486,6 +490,7 @@ static int _pinMode (int pin, int mode)
 			softToneCreate(origPin);
 			break;
 		case PWM_OUTPUT:
+			setIomuxMode(origPin, M1_FUNC_PWM);
 			pwmSetup(origPin);
 			break;
 		default:
diff --git a/wiringPi/odroidm1.h b/wiringPi/odroidm1.h
index f98abee..b272967 100644
--- a/wiringPi/odroidm1.h
+++ b/wiringPi/odroidm1.h
@@ -36,6 +36,7 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 #define GPIO_SIZE	32
 
 #define M1_FUNC_GPIO 0
+#define M1_FUNC_PWM 1
 
 // GPIO[0]
 #define M1_GPIO_0_BASE	0xFDD60000
diff --git a/wiringPi/odroidm1s.c b/wiringPi/odroidm1s.c
index d0856cc..6a75195 100644
--- a/wiringPi/odroidm1s.c
+++ b/wiringPi/odroidm1s.c
@@ -427,13 +427,18 @@ static int setIomuxMode (int pin, int mode)
 	bank = (bank != 0);
 	data = *(grf[bank] + regOffset);
 
-	// Common IOMUX Funtion 1 : GPIO (3'h0)
 	switch (mode) {
 	case M1_FUNC_GPIO: // Common IOMUX Function 1_GPIO (3'h0)
 		data &= ~(0x7 << ((groupOffset % 4) * 4)); // ~0x07 = 3'h0
 		data |= (0x7 << ((groupOffset % 4) * 4 + 16)); // write_mask
 		*(grf[bank] + regOffset) = data;
 		break;
+	case M1_FUNC_PWM: // gpio0_B5/B6: 3'h100 gpio0_C2: 3'h001
+		data |= (group < 2 ? (0x4 << ((groupOffset % 4) * 4)) : (0x1 << ((groupOffset % 4) * 4)));
+		data &= (group < 2 ? ~(0x3 << ((groupOffset % 4) * 4)) : ~(0x6 << ((groupOffset % 4) * 4)));
+		data |= (0x7 << ((groupOffset % 4) * 4 + 16)); // write_mask
+		*(grf[bank] + regOffset) = data;
+		break;
 	default:
 		break;
 	}
@@ -462,9 +467,7 @@ static int _pinMode (int pin, int mode)
 	pwmRelease(origPin);
 	softPwmStop(origPin);
 	softToneStop(origPin);
-
 	setClkState(bank, M1_CLK_ENABLE);
-	setIomuxMode(origPin, M1_FUNC_GPIO);
 
 	data = *(gpio[bank] + regOffset);
 
@@ -475,6 +478,7 @@ static int _pinMode (int pin, int mode)
 			_pullUpDnControl(origPin, mode);
 			__attribute__((fallthrough));
 		case OUTPUT:
+			setIomuxMode(origPin, M1_FUNC_GPIO);
 			mode = (mode == OUTPUT);
 			data &= ~(1 << gpioToShiftRegBy16(pin));
 			data |=(mode << gpioToShiftRegBy16(pin));
@@ -488,6 +492,7 @@ static int _pinMode (int pin, int mode)
 			softToneCreate(origPin);
 			break;
 		case PWM_OUTPUT:
+			setIomuxMode(origPin, M1_FUNC_PWM);
 			pwmSetup(origPin);
 			break;
 		default: