driver: px4_mldev: 電源連動制御の動作モードを指定できるオプションを追加

driver/px4_device.c

@@ -276,30 +276,36 @@ static int px4_chrdev_open(struct ptx_chrdev *chrdev)
 	struct ptx_chrdev_group *chrdev_group = chrdev->parent;
 	struct px4_chrdev *chrdev4 = chrdev->priv;
 	struct px4_device *px4 = chrdev4->parent;
+	bool need_init = false;
 
 	dev_dbg(px4->dev,
 		"px4_chrdev_open %u:%u\n", chrdev_group->id, chrdev->id);
 
 	mutex_lock(&px4->lock);
 
-	if (!px4->open_count) {
-		if (px4->mldev) {
-			ret = px4_mldev_set_power(px4->mldev, px4, true);
-			if (ret) {
-				dev_err(px4->dev,
-					"px4_chrdev_open %u:%u: px4_mldev_set_power(true) failed. (ret: %d)\n",
-					chrdev_group->id, chrdev->id, ret);
-				goto fail_backend_power;
-			}
-		} else {
-			ret = px4_backend_set_power(px4, true);
-			if (ret) {
-				dev_err(px4->dev,
-					"px4_chrdev_open %u:%u: px4_backend_set_power(true) failed. (ret: %d)\n",
-					chrdev_group->id, chrdev->id, ret);
-				goto fail_backend_power;
-			}
+	if (px4->mldev) {
+		ret = px4_mldev_set_power(px4->mldev, px4, chrdev->id, true, &need_init);
+		if (ret) {
+			dev_err(px4->dev,
+				"px4_chrdev_open %u:%u: px4_mldev_set_power(true) failed. (ret: %d)\n",
+				chrdev_group->id, chrdev->id, ret);
+			goto fail_backend_power;
 		}
+	} else if (!px4->open_count) {
+		ret = px4_backend_set_power(px4, true);
+		if (ret) {
+			dev_err(px4->dev,
+				"px4_chrdev_open %u:%u: px4_backend_set_power(true) failed. (ret: %d)\n",
+				chrdev_group->id, chrdev->id, ret);
+			goto fail_backend_power;
+		}
+		need_init = true;
+	}
+
+	if (need_init) {
+		dev_dbg(px4->dev,
+			"px4_chrdev_open %u:%u: init\n",
+			chrdev_group->id, chrdev->id);
 
 		ret = px4_backend_init(px4);
 		if (ret) {
@@ -507,7 +513,7 @@ fail_backend:
 fail_backend_init:
 	if (!px4->open_count) {
 		if (px4->mldev)
-			px4_mldev_set_power(px4->mldev, px4, false);
+			px4_mldev_set_power(px4->mldev, px4, chrdev->id, false, NULL);
 		else
 			px4_backend_set_power(px4, false);
 	}
@@ -542,9 +548,7 @@ static int px4_chrdev_release(struct ptx_chrdev *chrdev)
 	px4->open_count--;
 	if (!px4->open_count) {
 		px4_backend_term(px4);
-		if (px4->mldev)
-			px4_mldev_set_power(px4->mldev, px4, false);
-		else
+		if (!px4->mldev)
 			px4_backend_set_power(px4, false);
 	} else if (atomic_read(&px4->available)) {
 		/* sleep tuners */
@@ -566,6 +570,9 @@ static int px4_chrdev_release(struct ptx_chrdev *chrdev)
 		}
 	}
 
+	if (px4->mldev)
+		px4_mldev_set_power(px4->mldev, px4, chrdev->id, false, NULL);
+
 	if (kref_put(&px4->kref, px4_device_release))
 		return 0;
 
@@ -1288,8 +1295,8 @@ int px4_device_init(struct px4_device *px4, struct device *dev,
 		if (px4_mldev_search(px4->serial.serial_number, &px4->mldev))
 			ret = px4_mldev_add(px4->mldev, px4);
 		else
-			ret = px4_mldev_alloc(&px4->mldev, px4,
-					      px4_backend_set_power);
+			ret = px4_mldev_alloc(&px4->mldev, PX4_MLDEV_ALL_MODE,
+					      px4, px4_backend_set_power);
 
 		if (ret)
 			goto fail_device;

driver/px4_mldev.c

@@ -15,7 +15,10 @@
 static LIST_HEAD(px4_mldev_list);
 static DEFINE_MUTEX(px4_mldev_glock);
 
-static unsigned int px4_mldev_get_power_count(struct px4_mldev *mldev);
+static bool px4_mldev_get_chrdev_status(struct px4_mldev *mldev,
+				       unsigned int dev_id);
+static bool px4_mldev_is_power_interlocking_required(struct px4_mldev *mldev,
+						     unsigned int dev_id);
 
 bool px4_mldev_search(unsigned long long serial_number,
 		      struct px4_mldev **mldev)
@@ -36,10 +39,11 @@ bool px4_mldev_search(unsigned long long serial_number,
 	return (*mldev) ? true : false;
 }
 
-int px4_mldev_alloc(struct px4_mldev **mldev, struct px4_device *px4,
+int px4_mldev_alloc(struct px4_mldev **mldev,  enum px4_mldev_mode mode,
+		    struct px4_device *px4,
 		    int (*backend_set_power)(struct px4_device *, bool))
 {
-	int i;
+	int i, j;
 	unsigned int dev_id = px4->serial.dev_id - 1;
 	struct px4_mldev *m;
 
@@ -56,10 +60,13 @@ int px4_mldev_alloc(struct px4_mldev **mldev, struct px4_device *px4,
 
 	kref_init(&m->kref);
 	mutex_init(&m->lock);
+	m->mode = mode;
 	m->serial_number = px4->serial.serial_number;
 	for (i = 0; i < 2; i++) {
 		m->dev[i] = (i == dev_id) ? px4 : NULL;
 		m->power_state[i] = false;
+		for (j = 0; j < 4; j++)
+			m->chrdev_state[i][j] = false;
 	}
 	m->backend_set_power = backend_set_power;
 
@@ -91,7 +98,7 @@ static void px4_mldev_release(struct kref *kref)
 
 int px4_mldev_add(struct px4_mldev *mldev, struct px4_device *px4)
 {
-	int ret = 0;
+	int ret = 0, i;
 	unsigned int dev_id = px4->serial.dev_id - 1;
 
 	dev_dbg(px4->dev,
@@ -113,15 +120,20 @@ int px4_mldev_add(struct px4_mldev *mldev, struct px4_device *px4)
 		goto exit;
 	}
 
-	if (px4_mldev_get_power_count(mldev))
-		ret = mldev->backend_set_power(px4, true);
-
-	if (ret)
-		goto exit;
-
-	kref_get(&mldev->kref);
-	mldev->dev[dev_id] = px4;
 	mldev->power_state[dev_id] = false;
+	for (i = 0; i < 4; i++)
+		mldev->chrdev_state[dev_id][i] = false;
+
+	if (px4_mldev_is_power_interlocking_required(mldev, (dev_id) ? 0 : 1)) {
+		ret = mldev->backend_set_power(px4, true);
+		if (ret)
+			goto exit;
+
+		mldev->power_state[dev_id] = true;
+	}
+
+	mldev->dev[dev_id] = px4;
+	kref_get(&mldev->kref);
 
 exit:
 	mutex_unlock(&mldev->lock);
@@ -130,7 +142,9 @@ exit:
 
 int px4_mldev_remove(struct px4_mldev *mldev, struct px4_device *px4)
 {
+	int i;
 	unsigned int dev_id = px4->serial.dev_id - 1;
+	unsigned int other_dev_id = (dev_id) ? 1 : 0;
 
 	dev_dbg(px4->dev,
 		"px4_mldev_remove: serial_number: %014llu, dev_id: %u\n",
@@ -146,11 +160,20 @@ int px4_mldev_remove(struct px4_mldev *mldev, struct px4_device *px4)
 		return -EINVAL;
 	}
 
-	if (px4_mldev_get_power_count(mldev))
+	if (mldev->power_state[dev_id])
 		mldev->backend_set_power(px4, false);
 
 	mldev->dev[dev_id] = NULL;
 	mldev->power_state[dev_id] = false;
+	for (i = 0; i < 4; i++)
+		mldev->chrdev_state[dev_id][i] = false;
+
+	if (mldev->dev[other_dev_id] &&
+	    !px4_mldev_get_chrdev_status(mldev, other_dev_id) &&
+	    mldev->power_state[other_dev_id]) {
+		mldev->backend_set_power(mldev->dev[other_dev_id], false);
+		mldev->power_state[other_dev_id] = false;
+	}
 
 	if (kref_put(&mldev->kref, px4_mldev_release))
 		return 0;
@@ -159,54 +182,110 @@ int px4_mldev_remove(struct px4_mldev *mldev, struct px4_device *px4)
 	return 0;
 }
 
-static unsigned int px4_mldev_get_power_count(struct px4_mldev *mldev)
+static bool px4_mldev_get_chrdev_status(struct px4_mldev *mldev,
+				       unsigned int dev_id)
 {
-	return (!!mldev->power_state[0]) + (!!mldev->power_state[1]);
+	bool *state = mldev->chrdev_state[dev_id];
+	return (state[0] || state[1] || state[2] || state[3]);
+}
+
+static bool px4_mldev_is_power_interlocking_required(struct px4_mldev *mldev,
+						     unsigned int dev_id)
+{
+	bool ret = false;
+	bool *state = mldev->chrdev_state[dev_id];
+
+	switch (mldev->mode) {
+	case PX4_MLDEV_S_ONLY_MODE:
+		ret = state[0] || state[1];
+		break;
+
+	case PX4_MLDEV_S0_ONLY_MODE:
+		ret = state[0];
+		break;
+
+	case PX4_MLDEV_S1_ONLY_MODE:
+		ret = state[1];
+		break;
+
+	default:
+		ret = state[0] || state[1] || state[2] || state[3];
+		break;
+	}
+
+	return ret;
 }
 
 int px4_mldev_set_power(struct px4_mldev *mldev, struct px4_device *px4,
-			bool state)
+			unsigned int chrdev_id, bool state, bool *first)
 {
-	int ret = 0, i;
+	int ret = 0;
 	unsigned int dev_id = px4->serial.dev_id - 1;
+	unsigned int other_dev_id = (dev_id) ? 0 : 1;
 
 	dev_dbg(px4->dev,
-		"px4_mldev_set_power: serial_number: %014llu, dev_id: %u, state: %s\n",
-		mldev->serial_number, dev_id + 1, (state) ? "true" : "false");
+		"px4_mldev_set_power: serial_number: %014llu, dev_id: %u, chrdev_id: %u state: %s\n",
+		mldev->serial_number, dev_id, chrdev_id,
+		(state) ? "true" : "false");
 	dev_dbg(px4->dev,
 		"px4_mldev_set_power: power_state: %s, %s\n",
 		(mldev->power_state[0]) ? "true" : "false",
 		(mldev->power_state[1]) ? "true" : "false");
+	dev_dbg(px4->dev,
+		"px4_mldev_set_power: chrdev_state[%u][%u]: %s\n",
+		dev_id, chrdev_id, 
+		(mldev->chrdev_state[dev_id][chrdev_id]) ? "true" : "false");
 
-	if (dev_id > 1)
+	if (dev_id > 1 || chrdev_id > 3)
 		return -EINVAL;
 
 	mutex_lock(&mldev->lock);
 
-	if (!mldev->dev[dev_id]) {
+	if (mldev->dev[dev_id] != px4) {
 		ret = -EINVAL;
 		goto exit;
 	}
 
-	if (mldev->power_state[dev_id] == state)
+	if (mldev->chrdev_state[dev_id][chrdev_id] == state)
 		goto exit;
 
 	if (!state)
-		mldev->power_state[dev_id] = false;
+		mldev->chrdev_state[dev_id][chrdev_id] = false;
 
-	if (!px4_mldev_get_power_count(mldev)) {
-		for (i = 0; i < 2; i++) {
-			if (!mldev->dev[i])
-				continue;
+	if (!px4_mldev_get_chrdev_status(mldev, dev_id)) {
+		if (mldev->power_state[dev_id] != state &&
+		    (state || !px4_mldev_is_power_interlocking_required(mldev, other_dev_id))) {
+			ret = mldev->backend_set_power(mldev->dev[dev_id],
+						       state);
+			if (ret && state)
+				goto exit;
 
-			ret = mldev->backend_set_power(mldev->dev[i], state);
-			if (ret)
-				break;
+			mldev->power_state[dev_id] = state;
 		}
+
+		if (state && first)
+			*first = true;
 	}
 
-	if (state && !ret)
-		mldev->power_state[dev_id] = true;
+	if (state)
+		mldev->chrdev_state[dev_id][chrdev_id] = true;
+
+	if (mldev->dev[other_dev_id]) {
+		bool interlocking = px4_mldev_is_power_interlocking_required(mldev, dev_id);
+
+		dev_dbg(px4->dev,
+			"px4_mldev_set_power: interlocking: %s\n",
+			(interlocking) ? "true" : "false");
+
+		if (interlocking == state &&
+		    mldev->power_state[other_dev_id] != interlocking &&
+		    (state || !px4_mldev_get_chrdev_status(mldev, other_dev_id))) {
+			ret = mldev->backend_set_power(mldev->dev[other_dev_id],
+						       state);
+			if (!ret || !state)
+				mldev->power_state[other_dev_id] = state;
+		}
+	}
 
 exit:
 	mutex_unlock(&mldev->lock);

driver/px4_mldev.h

@@ -15,23 +15,33 @@
 
 #include "px4_device.h"
 
+enum px4_mldev_mode {
+	PX4_MLDEV_ALL_MODE = 0,
+	PX4_MLDEV_S_ONLY_MODE,
+	PX4_MLDEV_S0_ONLY_MODE,
+	PX4_MLDEV_S1_ONLY_MODE,
+};
+
 struct px4_mldev {
 	struct kref kref;
 	struct list_head list;
 	struct mutex lock;
+	enum px4_mldev_mode mode;
 	unsigned long long serial_number;
 	struct px4_device *dev[2];
 	bool power_state[2];
+	bool chrdev_state[2][4];
 	int (*backend_set_power)(struct px4_device *px4, bool state);
 };
 
 bool px4_mldev_search(unsigned long long serial_number,
 		      struct px4_mldev **mldev);
-int px4_mldev_alloc(struct px4_mldev **mldev, struct px4_device *px4,
+int px4_mldev_alloc(struct px4_mldev **mldev, enum px4_mldev_mode mode,
+		    struct px4_device *px4,
 		    int (*backend_set_power)(struct px4_device *, bool));
 int px4_mldev_add(struct px4_mldev *mldev, struct px4_device *px4);
 int px4_mldev_remove(struct px4_mldev *mldev, struct px4_device *px4);
 int px4_mldev_set_power(struct px4_mldev *mldev, struct px4_device *px4,
-			bool state);
+			unsigned int chrdev_id, bool state, bool *first);
 
 #endif