From 72e781d0eb734ecc9c254f80dc35f20b0a5e3e92 Mon Sep 17 00:00:00 2001
From: Daniel Beer <dlbeer@gmail.com>
Date: Fri, 1 Feb 2013 08:09:38 +1300
Subject: [PATCH] fet_core: Add workaround for CC5137 replug problem.

This chip needs to be replugged if the the shutdown procedure (reset and
close) runs. We look for the chip's ID and skip the procedure if it
matches.
---
 drivers/device.c   |  5 +++++
 drivers/device.h   |  5 +++++
 drivers/fet_core.c | 34 ++++++++++++++++++++--------------
 3 files changed, 30 insertions(+), 14 deletions(-)

diff --git a/drivers/device.c b/drivers/device.c
index 33f5e48..71e84c7 100644
--- a/drivers/device.c
+++ b/drivers/device.c
@@ -144,3 +144,8 @@ int device_erase(device_erase_type_t et, address_t addr)
 
 	return device_default->type->erase(device_default, et, addr);
 }
+
+int device_needs_skip_close(device_t dev)
+{
+	return (dev->dev_id[0] == 0x51) && (dev->dev_id[1] == 0x37);
+}
diff --git a/drivers/device.h b/drivers/device.h
index 5b74efd..27d4ae7 100644
--- a/drivers/device.h
+++ b/drivers/device.h
@@ -140,6 +140,11 @@ int device_probe_id(device_t dev);
  */
 int device_is_fram(device_t dev);
 
+/* Determine, from the device ID bytes, whether this chip has problems
+ * with the FET close procedure.
+ */
+int device_needs_skip_close(device_t dev);
+
 /* Set or clear a breakpoint. The index of the modified entry is
  * returned, or -1 if no free entries were available. The modified
  * entry is flagged so that it will be reloaded on the next run.
diff --git a/drivers/fet_core.c b/drivers/fet_core.c
index cb7c1ac..1eceb59 100644
--- a/drivers/fet_core.c
+++ b/drivers/fet_core.c
@@ -709,22 +709,28 @@ void fet_destroy(device_t dev_base)
 {
 	struct fet_device *dev = (struct fet_device *)dev_base;
 
-	/* The second argument to C_RESET is a boolean which specifies
-	 * whether the chip should run or not. The final argument is
-	 * also a boolean. Setting it non-zero is required to get the
-	 * RST pin working on the G2231, but it must be zero on the
-	 * FR5739, or else the value of the reset vector gets set to
-	 * 0xffff at the start of the next JTAG session.
-	 */
-	if (fet_proto_xfer(&dev->proto, C_RESET, NULL, 0, 3, FET_RESET_ALL, 1,
-			   !device_is_fram(dev_base)) < 0)
-		printc_err("fet: final reset failed\n");
+	if (device_needs_skip_close(dev_base)) {
+		printc_dbg("Skipping close procedure");
+	} else {
+		/* The second argument to C_RESET is a boolean which
+		 * specifies whether the chip should run or not. The
+		 * final argument is also a boolean. Setting it non-zero
+		 * is required to get the RST pin working on the G2231,
+		 * but it must be zero on the FR5739, or else the value
+		 * of the reset vector gets set to 0xffff at the start
+		 * of the next JTAG session.
+		 */
+		if (fet_proto_xfer(&dev->proto, C_RESET, NULL, 0, 3,
+				   FET_RESET_ALL, 1,
+				   !device_is_fram(dev_base)) < 0)
+			printc_err("fet: final reset failed\n");
 
-	if (fet_proto_xfer(&dev->proto, C_CLOSE, NULL, 0, 1, 0) < 0)
-		printc_err("fet: close command failed\n");
+		if (fet_proto_xfer(&dev->proto, C_CLOSE, NULL, 0, 1, 0) < 0)
+			printc_err("fet: close command failed\n");
 
-	if (dev->base.power_buf)
-		powerbuf_free(dev->base.power_buf);
+		if (dev->base.power_buf)
+			powerbuf_free(dev->base.power_buf);
+	}
 
 	dev->proto.transport->ops->destroy(dev->proto.transport);
 	free(dev);