[05/11] fsp-sensor: rework device tree for sensors

diff --git a/hw/fsp/fsp-sensor.c b/hw/fsp/fsp-sensor.c
index 0287ad5ac047..dd178138b005 100644
--- a/hw/fsp/fsp-sensor.c
+++ b/hw/fsp/fsp-sensor.c
@@ -35,7 +35,8 @@ 
 #include <device.h>
 #include <spcn.h>
 #include <opal-msg.h>
-#include<errorlog.h>
+#include <errorlog.h>
+#include <sensor.h>
 
 #define INVALID_DATA	((uint32_t)-1)
 
@@ -228,14 +229,42 @@  static void queue_msg_for_delivery(int rc, struct opal_sensor_data *attr);
  * --------------------------------------------------------------------------
  */
 
+
+/*
+ * When coming from a SENSOR_POWER modifier command, the resource id
+ * of a power supply is on one byte and misses a "subclass" byte
+ * (0x10). This routine adds it to be consistent with the PRS_STATUS
+ * modifier command.
+ */
+#define normalize_power_rid(rid) (0x1000|(rid))
+
+static uint32_t sensor_power_process_data(uint16_t rid,
+					struct sensor_power *power)
+{
+	int i;
+
+	if (!sensor_power_is_valid(power)) {
+		prlog(PR_TRACE, "Power Sensor data not valid\n");
+		return INVALID_DATA;
+	}
+
+	for (i = 0; i < sensor_power_count(power); i++) {
+		prlog(PR_TRACE, "Power[%d]: %d mW\n", i,
+		      power->supplies[i].milliwatts);
+		if (rid == normalize_power_rid(power->supplies[i].rid))
+			return power->supplies[i].milliwatts / 1000;
+	}
+
+	return 0;
+}
+
+
 static void fsp_sensor_process_data(struct opal_sensor_data *attr)
 {
 	uint8_t *sensor_buf_ptr = (uint8_t *)sensor_buffer;
 	uint32_t sensor_data = INVALID_DATA;
 	uint16_t sensor_mod_data[8];
-	int count, i;
-	uint8_t valid, nr_power;
-	uint32_t power;
+	int count;
 
 	for (count = 0; count < spcn_mod_data[attr->mod_index].entry_count;
 			count++) {
@@ -245,19 +274,9 @@  static void fsp_sensor_process_data(struct opal_sensor_data *attr)
 			/* TODO Support this modifier '0x14', if required */
 
 		} else if (spcn_mod_data[attr->mod_index].mod == SPCN_MOD_SENSOR_POWER) {
-			valid = sensor_buf_ptr[0];
-			if (valid & 0x80) {
-				nr_power = valid & 0x0f;
-				sensor_data = 0;
-				for (i=0; i < nr_power; i++) {
-					power = *(uint32_t *) &sensor_buf_ptr[2 + i * 5];
-					prlog(PR_TRACE, "Power[%d]: %d mW\n",
-					      i, power);
-					sensor_data += power/1000;
-				}
-			} else {
-				prlog(PR_TRACE, "Power Sensor data not valid\n");
-			}
+			sensor_data = sensor_power_process_data(attr->rid,
+					(struct sensor_power *) sensor_buf_ptr);
+			break;
 		} else if (sensor_mod_data[0] == attr->frc &&
 				sensor_mod_data[1] == attr->rid) {
 			switch (attr->spcn_attr) {
@@ -411,7 +430,7 @@  static int64_t fsp_sensor_send_read_request(struct opal_sensor_data *attr)
 	uint32_t align;
 	uint32_t cmd_header;
 
-	prlog(PR_INSANE, "Get the data for modifier [%d]\n",
+	prlog(PR_INSANE, "Get the data for modifier [%x]\n",
 	      spcn_mod_data[attr->mod_index].mod);
 
 	if (spcn_mod_data[attr->mod_index].mod == SPCN_MOD_PROC_JUNC_TEMP) {
@@ -457,24 +476,66 @@  static int64_t fsp_sensor_send_read_request(struct opal_sensor_data *attr)
 	return OPAL_ASYNC_COMPLETION;
 }
 
-static int64_t parse_sensor_id(uint32_t id, struct opal_sensor_data *attr)
+/*
+ * These are the resources we know about and for which we provide a
+ * mapping in the device tree to capture data from the OS. Just
+ * discard the other ones for the moment.
+ */
+static inline bool sensor_frc_is_valid(uint16_t frc)
+{
+	switch (frc) {
+	case SENSOR_FRC_POWER_SUPPLY:
+	case SENSOR_FRC_COOLING_FAN:
+	case SENSOR_FRC_AMB_TEMP:
+		return true;
+	default:
+		return false;
+	}
+}
+
+/*
+ * Each attribute of a resource needs a request to the FSP to capture
+ * its data. The routine below provides the mapping between the
+ * attribute and the PRS command modifier to use.
+ *
+ *	resource        | data   |  thrs  |
+ *	----------------+--------+--------+
+ *	power_supply    | POWER  |        |
+ *	----------------+--------+--------+
+ *	amb-temp        | DATA   |        |
+ *	                |        | PARAM  |
+ *	----------------+--------+--------+
+ *	fan             | DATA   |        |
+ *	                |        | PARAM  |
+ *	                |        |        |
+ */
+static int64_t parse_sensor_id(uint32_t handler, struct opal_sensor_data *attr)
 {
 	uint32_t mod, index;
 
-	attr->spcn_attr = id >> 24;
-	if (attr->spcn_attr >= SENSOR_MAX)
+	attr->frc = sensor_get_frc(handler);
+	attr->rid = sensor_get_rid(handler);
+	attr->spcn_attr = sensor_get_attr(handler);
+
+	if (!sensor_frc_is_valid(attr->frc))
 		return OPAL_PARAMETER;
 
-	if (attr->spcn_attr <= SENSOR_ON_SUPPORTED)
-		mod = SPCN_MOD_PRS_STATUS_FIRST;
-	else if (attr->spcn_attr <= SENSOR_LOCATION)
+	/* now compute the PRS command modifier which will be used to
+	 * request a resource attribute from the FSP */
+	switch (attr->spcn_attr) {
+	case SENSOR_DATA:
+		if (attr->frc == SENSOR_FRC_POWER_SUPPLY)
+			mod = SPCN_MOD_SENSOR_POWER;
+		else
+			mod = SPCN_MOD_SENSOR_DATA_FIRST;
+		break;
+
+	case SENSOR_THRS:
 		mod = SPCN_MOD_SENSOR_PARAM_FIRST;
-	else if (attr->spcn_attr <= SENSOR_DATA)
-		mod = SPCN_MOD_SENSOR_DATA_FIRST;
-	else if (attr->spcn_attr <= SENSOR_POWER)
-		mod = SPCN_MOD_SENSOR_POWER;
-	else
+		break;
+	default:
 		return OPAL_PARAMETER;
+	}
 
 	for (index = 0; spcn_mod_data[index].mod != SPCN_MOD_LAST; index++) {
 		if (spcn_mod_data[index].mod == mod)
@@ -482,9 +543,6 @@  static int64_t parse_sensor_id(uint32_t id, struct opal_sensor_data *attr)
 	}
 
 	attr->mod_index = index;
-	attr->frc = (id >> 16) & 0xff;
-	attr->rid = id & 0xffff;
-
 	return 0;
 }
 
@@ -553,138 +611,165 @@  out:
 	return rc;
 }
 
-
-#define MAX_RIDS	64
 #define MAX_NAME	64
 
-static int get_index(uint16_t *prids, uint16_t rid)
+static struct dt_node *sensor_get_node(struct dt_node *sensors,
+				struct sensor_header *header)
 {
-	int index;
+	char name[MAX_NAME];
+	struct dt_node *node;
+
+	/*
+	 * Just use the resource class name and resource id. This
+	 * should be obvious enough for a node name.
+	 */
+	snprintf(name, sizeof(name), "%s@%x", frc_names[header->frc],
+		 header->rid);
+
+	/*
+	 * The same resources are reported by the different PRS
+	 * subcommands (PRS_STATUS, SENSOR_PARAM, SENSOR_DATA). So we
+	 * need to check that we did not already create the device
+	 * node.
+	 */
+	node = dt_find_by_path(sensors, name);
+	if (!node) {
+		prlog(PR_TRACE, "SENSOR: creating node %s\n", name);
+
+		node = dt_new(sensors, name);
+
+		dt_add_property_string(node, "compatible", "ibm,opal-sensor");
+	}
+	return node;
+}
+
+#define sensor_handler(header, attr_num) \
+	sensor_make_handler((header).frc, (header).rid, attr_num)
 
-	for (index = 0; prids[index] && index < MAX_RIDS; index++)
-		if (prids[index] == rid)
-			return index;
+static int add_sensor_prs(struct dt_node *sensors, struct sensor_prs *prs)
+{
+	struct dt_node *node;
 
-	if (index == MAX_RIDS)
+	node = sensor_get_node(sensors, &prs->header);
+	if (!node)
 		return -1;
 
-	prids[index] = rid;
-	return index;
+	return 0;
 }
 
-static void create_sensor_nodes(int index, uint16_t frc, uint16_t rid,
-		uint16_t *prids, struct dt_node *sensors)
+static int add_sensor_param(struct dt_node *sensors, struct sensor_param *param)
 {
-	char name[MAX_NAME];
-	struct dt_node *fs_node;
-	uint32_t value;
-	int rid_index;
-
-	switch (spcn_mod_data[index].mod) {
-	case SPCN_MOD_PRS_STATUS_FIRST:
-	case SPCN_MOD_PRS_STATUS_SUBS:
-		switch (frc) {
-		case SENSOR_FRC_POWER_SUPPLY:
-		case SENSOR_FRC_COOLING_FAN:
-			rid_index = get_index(prids, rid);
-			if (rid_index < 0)
-				break;
-			snprintf(name, MAX_NAME, "%s#%d-%s", frc_names[frc],
-					/* Start enumeration from 1 */
-					rid_index + 1,
-					spcn_mod_data[index].mod_attr[1].name);
-			fs_node = dt_new(sensors, name);
-			snprintf(name, MAX_NAME, "ibm,opal-sensor-%s",
-					frc_names[frc]);
-			dt_add_property_string(fs_node, "compatible", name);
-			value = spcn_mod_data[index].mod_attr[1].val << 24 |
-					(frc & 0xff) << 16 | rid;
-			dt_add_property_cells(fs_node, "sensor-id", value);
-			break;
-		default:
-			break;
-		}
-		break;
-	case SPCN_MOD_SENSOR_PARAM_FIRST:
-	case SPCN_MOD_SENSOR_PARAM_SUBS:
-	case SPCN_MOD_SENSOR_DATA_FIRST:
-	case SPCN_MOD_SENSOR_DATA_SUBS:
-		switch (frc) {
-		case SENSOR_FRC_POWER_SUPPLY:
-		case SENSOR_FRC_COOLING_FAN:
-		case SENSOR_FRC_AMB_TEMP:
-			rid_index = get_index(prids, rid);
-			if (rid_index < 0)
-				break;
-			snprintf(name, MAX_NAME, "%s#%d-%s", frc_names[frc],
-					/* Start enumeration from 1 */
-					rid_index + 1,
-					spcn_mod_data[index].mod_attr[0].name);
-			fs_node = dt_new(sensors, name);
-			snprintf(name, MAX_NAME, "ibm,opal-sensor-%s",
-					frc_names[frc]);
-			dt_add_property_string(fs_node, "compatible", name);
-			value = spcn_mod_data[index].mod_attr[0].val << 24 |
-					(frc & 0xff) << 16 | rid;
-			dt_add_property_cells(fs_node, "sensor-id", value);
-			break;
-		default:
-			break;
-		}
-		break;
+	struct dt_node *node;
 
-	case SPCN_MOD_SENSOR_POWER:
-		fs_node = dt_new(sensors, "power#1-data");
-		dt_add_property_string(fs_node, "compatible", "ibm,opal-sensor-power");
-		value = spcn_mod_data[index].mod_attr[0].val << 24;
-		dt_add_property_cells(fs_node, "sensor-id", value);
-		break;
+	node = sensor_get_node(sensors, &param->header);
+	if (!node)
+		return -1;
+
+	dt_add_property_string(node, "ibm,loc-code", param->location);
+	dt_add_property_cells(node, "sensor-threshold",
+			      sensor_handler(param->header, SENSOR_THRS));
+	return 0;
+}
+
+static int add_sensor_data(struct dt_node *sensors,
+				struct sensor_data *data)
+{
+	struct dt_node *node;
+
+	node = sensor_get_node(sensors, &data->header);
+	if (!node)
+		return -1;
+
+	dt_add_property_cells(node, "sensor-data",
+			      sensor_handler(data->header, SENSOR_DATA));
+	return 0;
+}
+
+static int add_sensor_power(struct dt_node *sensors, struct sensor_power *power)
+{
+	int i;
+	struct dt_node *node;
+
+	if (!sensor_power_is_valid(power))
+		return -1;
+
+	for (i = 0; i < sensor_power_count(power); i++) {
+		struct sensor_header header = {
+			SENSOR_FRC_POWER_SUPPLY,
+			normalize_power_rid(power->supplies[i].rid)
+		};
+
+		node = sensor_get_node(sensors, &header);
+
+		prlog(PR_TRACE, "SENSOR: Power[%d] : %d mW\n",
+		      power->supplies[i].rid,
+		      power->supplies[i].milliwatts);
+
+		dt_add_property_cells(node, "sensor-data",
+				      sensor_handler(header, SENSOR_DATA));
 	}
+	return 0;
 }
 
 static void add_sensor_ids(struct dt_node *sensors)
 {
-	uint32_t MAX_FRC_NAMES = sizeof(frc_names) / sizeof(*frc_names);
 	uint8_t *sensor_buf_ptr = (uint8_t *)sensor_buffer;
-	uint16_t *prids[MAX_FRC_NAMES];
-	uint16_t sensor_frc, power_rid;
-	uint16_t sensor_mod_data[8];
-	uint32_t index, count;
-
-	for (index = 0; index < MAX_FRC_NAMES; index++)
-		prids[index] = zalloc(MAX_RIDS * sizeof(**prids));
-
-	for (index = 0; spcn_mod_data[index].mod != SPCN_MOD_LAST; index++) {
-		if (spcn_mod_data[index].mod == SPCN_MOD_SENSOR_POWER) {
-			create_sensor_nodes(index, 0, 0, 0, sensors);
+	struct spcn_mod *smod;
+	int i;
+
+	for (smod = spcn_mod_data; smod->mod != SPCN_MOD_LAST; smod++) {
+		/*
+		 * SPCN_MOD_SENSOR_POWER (0x1C) has a different layout.
+		 */
+		if (smod->mod == SPCN_MOD_SENSOR_POWER) {
+			add_sensor_power(sensors,
+				      (struct sensor_power *) sensor_buf_ptr);
+
+			sensor_buf_ptr += smod->entry_size * smod->entry_count;
 			continue;
 		}
-		for (count = 0; count < spcn_mod_data[index].entry_count;
-				count++) {
-			if (spcn_mod_data[index].mod ==
-					SPCN_MOD_PROC_JUNC_TEMP) {
-				/* TODO Support this modifier '0x14', if
-				 * required */
-			} else {
-				memcpy((void *)sensor_mod_data, sensor_buf_ptr,
-						spcn_mod_data[index].entry_size);
-				sensor_frc = sensor_mod_data[0];
-				power_rid = sensor_mod_data[1];
-
-				if (sensor_frc < MAX_FRC_NAMES &&
-						frc_names[sensor_frc])
-					create_sensor_nodes(index, sensor_frc,
-							power_rid,
-							prids[sensor_frc],
-							sensors);
+
+		for (i = 0; i < smod->entry_count; i++) {
+			struct sensor_header *header =
+				(struct sensor_header *) sensor_buf_ptr;
+
+			if (!sensor_frc_is_valid(header->frc))
+				goto out_sensor;
+
+			switch (smod->mod) {
+			case SPCN_MOD_PROC_JUNC_TEMP:
+				/* TODO Support this modifier '0x14',
+				   if required */
+				break;
+
+			case SPCN_MOD_PRS_STATUS_FIRST:
+			case SPCN_MOD_PRS_STATUS_SUBS:
+				add_sensor_prs(sensors,
+					(struct sensor_prs *) header);
+				break;
+
+			case SPCN_MOD_SENSOR_PARAM_FIRST:
+			case SPCN_MOD_SENSOR_PARAM_SUBS:
+				add_sensor_param(sensors,
+					(struct sensor_param *) header);
+				break;
+
+			case SPCN_MOD_SENSOR_DATA_FIRST:
+			case SPCN_MOD_SENSOR_DATA_SUBS:
+				add_sensor_data(sensors,
+					(struct sensor_data *) header);
+
+				break;
+
+			default:
+				prerror("SENSOR: unknown modifier : %x\n",
+					smod->mod);
 			}
 
-			sensor_buf_ptr += spcn_mod_data[index].entry_size;
+out_sensor:
+			sensor_buf_ptr += smod->entry_size;
 		}
 	}
-
-	for (index = 0; index < MAX_FRC_NAMES; index++)
-		free(prids[index]);
 }
 
 static void add_opal_sensor_node(void)