Add retry on error and tidy of temperature driver

bspratt · Feb 24, 2013 · 6575037 · 6575037
1 parent 3b30895
commit 6575037
Showing 1 changed file with 27 additions and 51 deletions.
diff --git a/drivers/thermal/bcm2835-thermal.c b/drivers/thermal/bcm2835-thermal.c
@@ -33,7 +33,6 @@
 #define print_debug(fmt,...)
 #endif
 #define print_err(fmt,...) printk(KERN_ERR "%s:%s:%d: "fmt"\n", MODULE_NAME, __func__,__LINE__, ##__VA_ARGS__)
-#define print_info(fmt,...) printk(KERN_INFO "%s: "fmt"\n", MODULE_NAME, ##__VA_ARGS__)
 
 #define VC_TAG_GET_TEMP 0x00030006
 #define VC_TAG_GET_MAX_TEMP 0x0003000A
@@ -66,77 +65,54 @@ struct bcm2835_thermal_data {
 	struct vc_msg msg;
 };
 
-/* --- PROTOTYPES --- */
-static int bcm2835_get_temp(struct thermal_zone_device *thermal_dev, unsigned long *);
-static int bcm2835_get_max_temp(struct thermal_zone_device *thermal_dev, int, unsigned long *);
-static int bcm2835_get_trip_type(struct thermal_zone_device *thermal_dev, int trip_num, enum thermal_trip_type *trip_type);
-static int bcm2835_get_mode(struct thermal_zone_device *thermal_dev, enum thermal_device_mode *dev_mode);
-
 /* --- GLOBALS --- */
 static struct bcm2835_thermal_data bcm2835_data;
 
 /* Thermal Device Operations */
 static struct thermal_zone_device_ops ops;
 
 /* --- FUNCTIONS --- */
-static int bcm2835_get_max_temp(struct thermal_zone_device *thermal_dev, int trip_num, unsigned long *temp)
-{
-	int result;
 
+static int bcm2835_get_temp_or_max(struct thermal_zone_device *thermal_dev, unsigned long *temp, unsigned tag_id)
+{
+	int result = -1, retry = 3;
 	print_debug("IN");
 
-	/* wipe all previous message data */
-	memset(&bcm2835_data.msg, 0, sizeof bcm2835_data.msg);
-
-	/* prepare message */
-	bcm2835_data.msg.msg_size = sizeof bcm2835_data.msg;
-	bcm2835_data.msg.tag.buffer_size = 8;
-	bcm2835_data.msg.tag.tag_id = VC_TAG_GET_MAX_TEMP;
-
-	/* send the message */
-	result = bcm_mailbox_property(&bcm2835_data.msg, sizeof bcm2835_data.msg);
+	*temp = 0;
+	while (result != 0 && retry-- > 0) {
+		/* wipe all previous message data */
+		memset(&bcm2835_data.msg, 0, sizeof bcm2835_data.msg);
+
+		/* prepare message */
+		bcm2835_data.msg.msg_size = sizeof bcm2835_data.msg;
+		bcm2835_data.msg.tag.buffer_size = 8;
+		bcm2835_data.msg.tag.tag_id = tag_id;
+
+		/* send the message */
+		result = bcm_mailbox_property(&bcm2835_data.msg, sizeof bcm2835_data.msg);
+		print_debug("Got %stemperature as %u (%d,%x)\n", tag_id==VC_TAG_GET_MAX_TEMP ? "max ":"", (uint)bcm2835_data.msg.tag.val, result, bcm2835_data.msg.request_code);
+		if (!(bcm2835_data.msg.request_code & 0x80000000))
+			result = -1;
+	}
 
 	/* check if it was all ok and return the rate in milli degrees C */
-	if (result == 0 && (bcm2835_data.msg.request_code & 0x80000000))
+	if (result == 0)
 		*temp = (uint)bcm2835_data.msg.tag.val;
-	#ifdef THERMAL_DEBUG_ENABLE
 	else
-		print_debug("Failed to get temperature!");
-	#endif
-	print_debug("Got temperature as %u",(uint)*temp);
+		print_err("Failed to get temperature! (%x:%d)\n", tag_id, result);
 	print_debug("OUT");
-	return 0;
+	return result;
 }
 
 static int bcm2835_get_temp(struct thermal_zone_device *thermal_dev, unsigned long *temp)
 {
-	int result;
-
-	print_debug("IN");
-
-	/* wipe all previous message data */
-	memset(&bcm2835_data.msg, 0, sizeof bcm2835_data.msg);
-
-	/* prepare message */
-	bcm2835_data.msg.msg_size = sizeof bcm2835_data.msg;
-	bcm2835_data.msg.tag.buffer_size = 8;
-	bcm2835_data.msg.tag.tag_id = VC_TAG_GET_TEMP;
-
-	/* send the message */
-	result = bcm_mailbox_property(&bcm2835_data.msg, sizeof bcm2835_data.msg);
-
-	/* check if it was all ok and return the rate in milli degrees C */
-	if (result == 0 && (bcm2835_data.msg.request_code & 0x80000000))
-		*temp = (uint)bcm2835_data.msg.tag.val;
-	#ifdef THERMAL_DEBUG_ENABLE
-	else
-		print_debug("Failed to get temperature!");
-	#endif
-	print_debug("Got temperature as %u",(uint)*temp);
-	print_debug("OUT");
-	return 0;
+	return bcm2835_get_temp_or_max(thermal_dev, temp, VC_TAG_GET_TEMP);
 }
 
+static int bcm2835_get_max_temp(struct thermal_zone_device *thermal_dev, int trip_num, unsigned long *temp)
+{
+	return bcm2835_get_temp_or_max(thermal_dev, temp, VC_TAG_GET_MAX_TEMP);
+}
 
 static int bcm2835_get_trip_type(struct thermal_zone_device * thermal_dev, int trip_num, enum thermal_trip_type *trip_type)
 {