From 73dbecadf1c3e01970d6836aae83ef3c9cc03f90 Mon Sep 17 00:00:00 2001
From: Eric Katzfey <53063038+katzfey@users.noreply.github.com>
Date: Wed, 19 Feb 2025 07:59:45 -0800
Subject: [PATCH] Qurt: changed the mutex for I2C for the Qurt platform from a
 single mutex to one for each bus. (#23531)

---
 src/lib/drivers/device/qurt/I2C.cpp | 40 ++++++++++++++++++++++++-----
 src/lib/drivers/device/qurt/I2C.hpp | 10 +++++++-
 2 files changed, 42 insertions(+), 8 deletions(-)

diff --git a/src/lib/drivers/device/qurt/I2C.cpp b/src/lib/drivers/device/qurt/I2C.cpp
index c2b70f66d3..a459dbbe81 100644
--- a/src/lib/drivers/device/qurt/I2C.cpp
+++ b/src/lib/drivers/device/qurt/I2C.cpp
@@ -55,7 +55,16 @@ I2C::_config_i2c_bus_func_t  I2C::_config_i2c_bus  = NULL;
 I2C::_set_i2c_address_func_t I2C::_set_i2c_address = NULL;
 I2C::_i2c_transfer_func_t    I2C::_i2c_transfer    = NULL;
 
-pthread_mutex_t I2C::_mutex = PTHREAD_MUTEX_INITIALIZER;
+// There is a mutex per I2C bus. A mutex isn't required with normal
+// use since per bus I2C accesses are made via work item tied to a per bus thread.
+// But it is here anyways in case someone decides to use the bus in some different
+// custom code that has a separate thread.
+struct I2C::_bus_mutex_t I2C::_bus_mutex[I2C::MAX_I2C_BUS] = {
+	{1, PTHREAD_MUTEX_INITIALIZER},
+	{2, PTHREAD_MUTEX_INITIALIZER},
+	{4, PTHREAD_MUTEX_INITIALIZER},
+	{5, PTHREAD_MUTEX_INITIALIZER}
+};
 
 I2C::I2C(uint8_t device_type, const char *name, const int bus, const uint16_t address, const uint32_t frequency) :
 	CDev(name, nullptr),
@@ -91,10 +100,27 @@ I2C::init()
 		goto out;
 	}
 
-	pthread_mutex_lock(&_mutex);
+	if (_mutex == nullptr) {
+		for (int i = 0; i < MAX_I2C_BUS; i++) {
+			if (get_device_bus() == _bus_mutex[i]._bus) {
+				_mutex = &_bus_mutex[i]._mutex;
+				break;
+			}
+		}
+
+		if (_mutex == nullptr) {
+			PX4_ERR("NULL i2c bus mutex");
+			goto out;
+
+		} else {
+			PX4_INFO("Set up I2C bus mutex for bus %d", get_device_bus());
+		}
+	}
+
+	pthread_mutex_lock(_mutex);
 	// Open the actual I2C device
 	_i2c_fd = _config_i2c_bus(get_device_bus(), get_device_address(), _frequency);
-	pthread_mutex_unlock(&_mutex);
+	pthread_mutex_unlock(_mutex);
 
 	if (_i2c_fd == PX4_ERROR) {
 		PX4_ERR("i2c init failed");
@@ -131,9 +157,9 @@ I2C::set_device_address(int address)
 	if ((_i2c_fd != PX4_ERROR) && (_set_i2c_address != NULL)) {
 		PX4_INFO("Set i2c address 0x%x, fd %d", address, _i2c_fd);
 
-		pthread_mutex_lock(&_mutex);
+		pthread_mutex_lock(_mutex);
 		_set_i2c_address(_i2c_fd, address);
-		pthread_mutex_unlock(&_mutex);
+		pthread_mutex_unlock(_mutex);
 
 		Device::set_device_address(address);
 	}
@@ -150,9 +176,9 @@ I2C::transfer(const uint8_t *send, const unsigned send_len, uint8_t *recv, const
 		do {
 			// PX4_INFO("transfer out %p/%u  in %p/%u", send, send_len, recv, recv_len);
 
-			pthread_mutex_lock(&_mutex);
+			pthread_mutex_lock(_mutex);
 			ret = _i2c_transfer(_i2c_fd, send, send_len, recv, recv_len);
-			pthread_mutex_unlock(&_mutex);
+			pthread_mutex_unlock(_mutex);
 
 			if (ret != PX4_ERROR) { break; }
 
diff --git a/src/lib/drivers/device/qurt/I2C.hpp b/src/lib/drivers/device/qurt/I2C.hpp
index 7f1d59bc0a..ccf5e1f5df 100644
--- a/src/lib/drivers/device/qurt/I2C.hpp
+++ b/src/lib/drivers/device/qurt/I2C.hpp
@@ -124,10 +124,18 @@ protected:
 private:
 	uint32_t		               _frequency{0};
 	int                            _i2c_fd{-1};
+	pthread_mutex_t               *_mutex{nullptr};
+
+	static const int MAX_I2C_BUS{4};
+
 	static _config_i2c_bus_func_t  _config_i2c_bus;
 	static _set_i2c_address_func_t _set_i2c_address;
 	static _i2c_transfer_func_t    _i2c_transfer;
-	static pthread_mutex_t         _mutex;
+
+	static struct _bus_mutex_t {
+		int _bus;
+		pthread_mutex_t _mutex{PTHREAD_MUTEX_INITIALIZER};
+	} _bus_mutex[MAX_I2C_BUS];
 };
 
 } // namespace device