drm/nv50/therm: implement temperature reading

Signed-off-by: Martin Peres <martin.peres@labri.fr>
Signed-off-by: Ben Skeggs <bskeggs@redhat.com>

drivers/gpu/drm/nouveau/core/subdev/therm/nv50.c

@@ -118,10 +118,36 @@ nv50_fan_pwm_clock(struct nouveau_therm *therm)
 	return pwm_clock;
 }
 
-int
+static void
+nv50_sensor_setup(struct nouveau_therm *therm)
+{
+	nv_mask(therm, 0x20010, 0x40000000, 0x0);
+	mdelay(20); /* wait for the temperature to stabilize */
+}
+
+static int
 nv50_temp_get(struct nouveau_therm *therm)
 {
-	return nv_rd32(therm, 0x20400);
+	struct nouveau_therm_priv *priv = (void *)therm;
+	struct nvbios_therm_sensor *sensor = &priv->bios_sensor;
+	int core_temp;
+
+	core_temp = nv_rd32(therm, 0x20014) & 0x3fff;
+
+	/* if the slope or the offset is unset, do no use the sensor */
+	if (!sensor->slope_div || !sensor->slope_mult ||
+	    !sensor->offset_num || !sensor->offset_den)
+	    return -ENODEV;
+
+	core_temp = core_temp * sensor->slope_mult / sensor->slope_div;
+	core_temp = core_temp + sensor->offset_num / sensor->offset_den;
+	core_temp = core_temp + sensor->offset_constant - 8;
+
+	/* reserve negative temperatures for errors */
+	if (core_temp < 0)
+		core_temp = 0;
+
+	return core_temp;
 }
 
 static int
@@ -149,13 +175,23 @@ nv50_therm_ctor(struct nouveau_object *parent,
 	return nouveau_therm_preinit(&priv->base.base);
 }
 
+static int
+nv50_therm_init(struct nouveau_object *object)
+{
+	struct nouveau_therm *therm = (void *)object;
+
+	nv50_sensor_setup(therm);
+
+	return _nouveau_therm_init(object);
+}
+
 struct nouveau_oclass
 nv50_therm_oclass = {
 	.handle = NV_SUBDEV(THERM, 0x50),
 	.ofuncs = &(struct nouveau_ofuncs) {
 		.ctor = nv50_therm_ctor,
 		.dtor = _nouveau_therm_dtor,
-		.init = _nouveau_therm_init,
+		.init = nv50_therm_init,
 		.fini = _nouveau_therm_fini,
 	},
 };