1 files changed, 109 insertions, 42 deletions

diff --git a/drivers/fsi/fsi-occ.c b/drivers/fsi/fsi-occ.c
index 7eaab1be0aa4..abdd37d5507f 100644
--- a/drivers/fsi/fsi-occ.c
+++ b/drivers/fsi/fsi-occ.c
@@ -44,6 +44,7 @@ struct occ {
 	struct device *sbefifo;
 	char name[32];
 	int idx;
+	bool platform_hwmon;
 	u8 sequence_number;
 	void *buffer;
 	void *client_buffer;
@@ -94,6 +95,7 @@ static int occ_open(struct inode *inode, struct file *file)
 	client->occ = occ;
 	mutex_init(&client->lock);
 	file->private_data = client;
+	get_device(occ->dev);
 
 	/* We allocate a 1-page buffer, make sure it all fits */
 	BUILD_BUG_ON((OCC_CMD_DATA_BYTES + 3) > PAGE_SIZE);
@@ -197,6 +199,7 @@ static int occ_release(struct inode *inode, struct file *file)
 {
 	struct occ_client *client = file->private_data;
 
+	put_device(client->occ->dev);
 	free_page((unsigned long)client->buffer);
 	kfree(client);
 
@@ -246,7 +249,7 @@ static int occ_verify_checksum(struct occ *occ, struct occ_response *resp,
 	if (checksum != checksum_resp) {
 		dev_err(occ->dev, "Bad checksum: %04x!=%04x\n", checksum,
 			checksum_resp);
-		return -EBADMSG;
+		return -EBADE;
 	}
 
 	return 0;
@@ -451,6 +454,14 @@ static int occ_trigger_attn(struct occ *occ)
 	return rc;
 }
 
+static bool fsi_occ_response_not_ready(struct occ_response *resp, u8 seq_no,
+				       u8 cmd_type)
+{
+	return resp->return_status == OCC_RESP_CMD_IN_PRG ||
+		resp->return_status == OCC_RESP_CRIT_INIT ||
+		resp->seq_no != seq_no || resp->cmd_type != cmd_type;
+}
+
 int fsi_occ_submit(struct device *dev, const void *request, size_t req_len,
 		   void *response, size_t *resp_len)
 {
@@ -461,10 +472,11 @@ int fsi_occ_submit(struct device *dev, const void *request, size_t req_len,
 	struct occ_response *resp = response;
 	size_t user_resp_len = *resp_len;
 	u8 seq_no;
+	u8 cmd_type;
 	u16 checksum = 0;
 	u16 resp_data_length;
 	const u8 *byte_request = (const u8 *)request;
-	unsigned long start;
+	unsigned long end;
 	int rc;
 	size_t i;
 
@@ -478,16 +490,25 @@ int fsi_occ_submit(struct device *dev, const void *request, size_t req_len,
 		return -EINVAL;
 	}
 
+	cmd_type = byte_request[1];
+
 	/* Checksum the request, ignoring first byte (sequence number). */
 	for (i = 1; i < req_len - 2; ++i)
 		checksum += byte_request[i];
 
-	mutex_lock(&occ->occ_lock);
+	rc = mutex_lock_interruptible(&occ->occ_lock);
+	if (rc)
+		return rc;
 
 	occ->client_buffer = response;
 	occ->client_buffer_size = user_resp_len;
 	occ->client_response_size = 0;
 
+	if (!occ->buffer) {
+		rc = -ENOENT;
+		goto done;
+	}
+
 	/*
 	 * Get a sequence number and update the counter. Avoid a sequence
 	 * number of 0 which would pass the response check below even if the
@@ -509,53 +530,66 @@ int fsi_occ_submit(struct device *dev, const void *request, size_t req_len,
 	if (rc)
 		goto done;
 
-	/* Read occ response header */
-	start = jiffies;
-	do {
+	end = jiffies + timeout;
+	while (true) {
+		/* Read occ response header */
 		rc = occ_getsram(occ, 0, resp, 8);
 		if (rc)
 			goto done;
 
-		if (resp->return_status == OCC_RESP_CMD_IN_PRG ||
-		    resp->return_status == OCC_RESP_CRIT_INIT ||
-		    resp->seq_no != seq_no) {
-			rc = -ETIMEDOUT;
-
-			if (time_after(jiffies, start + timeout)) {
-				dev_err(occ->dev, "resp timeout status=%02x "
-					"resp seq_no=%d our seq_no=%d\n",
+		if (fsi_occ_response_not_ready(resp, seq_no, cmd_type)) {
+			if (time_after(jiffies, end)) {
+				dev_err(occ->dev,
+					"resp timeout status=%02x seq=%d cmd=%d, our seq=%d cmd=%d\n",
 					resp->return_status, resp->seq_no,
-					seq_no);
+					resp->cmd_type, seq_no, cmd_type);
+				rc = -ETIMEDOUT;
 				goto done;
 			}
 
 			set_current_state(TASK_UNINTERRUPTIBLE);
 			schedule_timeout(wait_time);
-		}
-	} while (rc);
-
-	/* Extract size of response data */
-	resp_data_length = get_unaligned_be16(&resp->data_length);
+		} else {
+			/* Extract size of response data */
+			resp_data_length =
+				get_unaligned_be16(&resp->data_length);
+
+			/*
+			 * Message size is data length + 5 bytes header + 2
+			 * bytes checksum
+			 */
+			if ((resp_data_length + 7) > user_resp_len) {
+				rc = -EMSGSIZE;
+				goto done;
+			}
 
-	/* Message size is data length + 5 bytes header + 2 bytes checksum */
-	if ((resp_data_length + 7) > user_resp_len) {
-		rc = -EMSGSIZE;
-		goto done;
+			/*
+			 * Get the entire response including the header again,
+			 * in case it changed
+			 */
+			if (resp_data_length > 1) {
+				rc = occ_getsram(occ, 0, resp,
+						 resp_data_length + 7);
+				if (rc)
+					goto done;
+
+				if (!fsi_occ_response_not_ready(resp, seq_no,
+								cmd_type))
+					break;
+			} else {
+				break;
+			}
+		}
 	}
 
 	dev_dbg(dev, "resp_status=%02x resp_data_len=%d\n",
 		resp->return_status, resp_data_length);
 
-	/* Grab the rest */
-	if (resp_data_length > 1) {
-		/* already got 3 bytes resp, also need 2 bytes checksum */
-		rc = occ_getsram(occ, 8, &resp->data[3], resp_data_length - 1);
-		if (rc)
-			goto done;
-	}
+	rc = occ_verify_checksum(occ, resp, resp_data_length);
+	if (rc)
+		goto done;
 
 	occ->client_response_size = resp_data_length + 7;
-	rc = occ_verify_checksum(occ, resp, resp_data_length);
 
  done:
 	*resp_len = occ->client_response_size;
@@ -565,7 +599,7 @@ int fsi_occ_submit(struct device *dev, const void *request, size_t req_len,
 }
 EXPORT_SYMBOL_GPL(fsi_occ_submit);
 
-static int occ_unregister_child(struct device *dev, void *data)
+static int occ_unregister_platform_child(struct device *dev, void *data)
 {
 	struct platform_device *hwmon_dev = to_platform_device(dev);
 
@@ -574,12 +608,25 @@ static int occ_unregister_child(struct device *dev, void *data)
 	return 0;
 }
 
+static int occ_unregister_of_child(struct device *dev, void *data)
+{
+	struct platform_device *hwmon_dev = to_platform_device(dev);
+
+	of_device_unregister(hwmon_dev);
+	if (dev->of_node)
+		of_node_clear_flag(dev->of_node, OF_POPULATED);
+
+	return 0;
+}
+
 static int occ_probe(struct platform_device *pdev)
 {
 	int rc;
 	u32 reg;
+	char child_name[32];
 	struct occ *occ;
-	struct platform_device *hwmon_dev;
+	struct platform_device *hwmon_dev = NULL;
+	struct device_node *hwmon_node;
 	struct device *dev = &pdev->dev;
 	struct platform_device_info hwmon_dev_info = {
 		.parent = dev,
@@ -598,7 +645,11 @@ static int occ_probe(struct platform_device *pdev)
 	occ->version = (uintptr_t)of_device_get_match_data(dev);
 	occ->dev = dev;
 	occ->sbefifo = dev->parent;
-	occ->sequence_number = 1;
+	/*
+	 * Quickly derive a pseudo-random number from jiffies so that
+	 * re-probing the driver doesn't accidentally overlap sequence numbers.
+	 */
+	occ->sequence_number = (u8)((jiffies % 0xff) + 1);
 	mutex_init(&occ->occ_lock);
 
 	if (dev->of_node) {
@@ -634,10 +685,20 @@ static int occ_probe(struct platform_device *pdev)
 		return rc;
 	}
 
-	hwmon_dev_info.id = occ->idx;
-	hwmon_dev = platform_device_register_full(&hwmon_dev_info);
-	if (IS_ERR(hwmon_dev))
-		dev_warn(dev, "failed to create hwmon device\n");
+	hwmon_node = of_get_child_by_name(dev->of_node, hwmon_dev_info.name);
+	if (hwmon_node) {
+		snprintf(child_name, sizeof(child_name), "%s.%d", hwmon_dev_info.name, occ->idx);
+		hwmon_dev = of_platform_device_create(hwmon_node, child_name, dev);
+		of_node_put(hwmon_node);
+	}
+
+	if (!hwmon_dev) {
+		occ->platform_hwmon = true;
+		hwmon_dev_info.id = occ->idx;
+		hwmon_dev = platform_device_register_full(&hwmon_dev_info);
+		if (IS_ERR(hwmon_dev))
+			dev_warn(dev, "failed to create hwmon device\n");
+	}
 
 	return 0;
 }
@@ -646,11 +707,17 @@ static int occ_remove(struct platform_device *pdev)
 {
 	struct occ *occ = platform_get_drvdata(pdev);
 
-	kvfree(occ->buffer);
-
 	misc_deregister(&occ->mdev);
 
-	device_for_each_child(&pdev->dev, NULL, occ_unregister_child);
+	mutex_lock(&occ->occ_lock);
+	kvfree(occ->buffer);
+	occ->buffer = NULL;
+	mutex_unlock(&occ->occ_lock);
+
+	if (occ->platform_hwmon)
+		device_for_each_child(&pdev->dev, NULL, occ_unregister_platform_child);
+	else
+		device_for_each_child(&pdev->dev, NULL, occ_unregister_of_child);
 
 	ida_simple_remove(&occ_ida, occ->idx);