--- v1
+++ v3
@@ -4,48 +4,126 @@
 
 Signed-off-by: Roderick Colenbrander <roderick.colenbrander@sony.com>
 ---
- drivers/hid/hid-playstation.c | 59 +++++++++++++++++++++++++++++++++++
- 1 file changed, 59 insertions(+)
+ drivers/hid/hid-playstation.c | 269 ++++++++++++++++++++++++++++++++++
+ 1 file changed, 269 insertions(+)
 
 diff --git a/drivers/hid/hid-playstation.c b/drivers/hid/hid-playstation.c
-index 598d262e2a08..7e622b02ee30 100644
+index c5c450fc25db..cde7d7fe5bac 100644
 --- a/drivers/hid/hid-playstation.c
 +++ b/drivers/hid/hid-playstation.c
-@@ -56,9 +56,14 @@ struct ps_device {
+@@ -32,9 +32,19 @@ struct ps_device {
+ 	int (*parse_report)(struct ps_device *dev, struct hid_report *report, u8 *data, int size);
+ };
+ 
++/* Calibration data for playstation motion sensors. */
++struct ps_calibration_data {
++	int abs_code;
++	short bias;
++	int sens_numer;
++	int sens_denom;
++};
++
+ #define DS_INPUT_REPORT_USB			0x01
+ #define DS_INPUT_REPORT_USB_SIZE		64
+ 
++#define DS_FEATURE_REPORT_CALIBRATION		0x05
++#define DS_FEATURE_REPORT_CALIBRATION_SIZE	41
+ #define DS_FEATURE_REPORT_PAIRING_INFO		0x09
+ #define DS_FEATURE_REPORT_PAIRING_INFO_SIZE	20
+ 
+@@ -60,9 +70,35 @@ struct ps_device {
  #define DS_STATUS_CHARGING		GENMASK(7, 4)
  #define DS_STATUS_CHARGING_SHIFT	4
  
++/*
++ * Status of a DualSense touch point contact.
++ * Contact IDs, with highest bit set are 'inactive'
++ * and any associated data is then invalid.
++ */
++#define DS_TOUCH_POINT_INACTIVE BIT(7)
++
 +/* DualSense hardware limits */
++#define DS_ACC_RES_PER_G	8192
++#define DS_ACC_RANGE		(4*DS_ACC_RES_PER_G)
++#define DS_GYRO_RES_PER_DEG_S	1024
++#define DS_GYRO_RANGE		(2048*DS_GYRO_RES_PER_DEG_S)
 +#define DS_TOUCHPAD_WIDTH	1920
 +#define DS_TOUCHPAD_HEIGHT	1080
 +
  struct dualsense {
  	struct ps_device base;
  	struct input_dev *gamepad;
++	struct input_dev *sensors;
 +	struct input_dev *touchpad;
++
++	/* Calibration data for accelerometer and gyroscope. */
++	struct ps_calibration_data accel_calib_data[3];
++	struct ps_calibration_data gyro_calib_data[3];
++
++	/* Timestamp for sensor data */
++	bool sensor_timestamp_initialized;
++	uint32_t prev_sensor_timestamp;
++	uint32_t sensor_timestamp_us;
  };
  
  struct dualsense_touch_point {
-@@ -239,6 +244,34 @@ static struct input_dev *ps_gamepad_create(struct hid_device *hdev)
- 	return gamepad;
+@@ -272,6 +308,161 @@ static int ps_get_report(struct hid_device *hdev, uint8_t report_id, uint8_t *bu
+ 	return 0;
  }
  
++static struct input_dev *ps_sensors_create(struct hid_device *hdev, int accel_range, int accel_res,
++		int gyro_range, int gyro_res)
++{
++	struct input_dev *sensors;
++	int ret;
++
++	sensors = ps_allocate_input_dev(hdev, "Motion Sensors");
++	if (IS_ERR(sensors))
++		return ERR_CAST(sensors);
++
++	__set_bit(INPUT_PROP_ACCELEROMETER, sensors->propbit);
++	__set_bit(EV_MSC, sensors->evbit);
++	__set_bit(MSC_TIMESTAMP, sensors->mscbit);
++
++	/* Accelerometer */
++	input_set_abs_params(sensors, ABS_X, -accel_range, accel_range, 16, 0);
++	input_set_abs_params(sensors, ABS_Y, -accel_range, accel_range, 16, 0);
++	input_set_abs_params(sensors, ABS_Z, -accel_range, accel_range, 16, 0);
++	input_abs_set_res(sensors, ABS_X, accel_res);
++	input_abs_set_res(sensors, ABS_Y, accel_res);
++	input_abs_set_res(sensors, ABS_Z, accel_res);
++
++	/* Gyroscope */
++	input_set_abs_params(sensors, ABS_RX, -gyro_range, gyro_range, 16, 0);
++	input_set_abs_params(sensors, ABS_RY, -gyro_range, gyro_range, 16, 0);
++	input_set_abs_params(sensors, ABS_RZ, -gyro_range, gyro_range, 16, 0);
++	input_abs_set_res(sensors, ABS_RX, gyro_res);
++	input_abs_set_res(sensors, ABS_RY, gyro_res);
++	input_abs_set_res(sensors, ABS_RZ, gyro_res);
++
++	ret = input_register_device(sensors);
++	if (ret)
++		return ERR_PTR(ret);
++
++	return sensors;
++}
++
 +static struct input_dev *ps_touchpad_create(struct hid_device *hdev, int width, int height,
-+		int num_contacts)
++		unsigned int num_contacts)
 +{
 +	struct input_dev *touchpad;
 +	int ret;
 +
 +	touchpad = ps_allocate_input_dev(hdev, "Touchpad");
 +	if (IS_ERR(touchpad))
-+		return ERR_PTR(-ENOMEM);
++		return ERR_CAST(touchpad);
 +
 +	/* Map button underneath touchpad to BTN_LEFT. */
 +	input_set_capability(touchpad, EV_KEY, BTN_LEFT);
 +	__set_bit(INPUT_PROP_BUTTONPAD, touchpad->propbit);
 +
-+	input_set_abs_params(touchpad, ABS_MT_POSITION_X, 0, width, 0, 0);
-+	input_set_abs_params(touchpad, ABS_MT_POSITION_Y, 0, height, 0, 0);
++	input_set_abs_params(touchpad, ABS_MT_POSITION_X, 0, width - 1, 0, 0);
++	input_set_abs_params(touchpad, ABS_MT_POSITION_Y, 0, height - 1, 0, 0);
 +
 +	ret = input_mt_init_slots(touchpad, num_contacts, INPUT_MT_POINTER);
 +	if (ret)
@@ -58,31 +136,161 @@
 +	return touchpad;
 +}
 +
++static int dualsense_get_calibration_data(struct dualsense *ds)
++{
++	short gyro_pitch_bias, gyro_pitch_plus, gyro_pitch_minus;
++	short gyro_yaw_bias, gyro_yaw_plus, gyro_yaw_minus;
++	short gyro_roll_bias, gyro_roll_plus, gyro_roll_minus;
++	short gyro_speed_plus, gyro_speed_minus;
++	short acc_x_plus, acc_x_minus;
++	short acc_y_plus, acc_y_minus;
++	short acc_z_plus, acc_z_minus;
++	int speed_2x;
++	int range_2g;
++	int ret = 0;
++	uint8_t *buf;
++
++	buf = kzalloc(DS_FEATURE_REPORT_CALIBRATION_SIZE, GFP_KERNEL);
++	if (!buf)
++		return -ENOMEM;
++
++	ret = ps_get_report(ds->base.hdev, DS_FEATURE_REPORT_CALIBRATION, buf,
++			DS_FEATURE_REPORT_CALIBRATION_SIZE);
++	if (ret) {
++		hid_err(ds->base.hdev, "Failed to retrieve DualSense calibration info: %d\n", ret);
++		goto err_free;
++	}
++
++	gyro_pitch_bias  = get_unaligned_le16(&buf[1]);
++	gyro_yaw_bias    = get_unaligned_le16(&buf[3]);
++	gyro_roll_bias   = get_unaligned_le16(&buf[5]);
++	gyro_pitch_plus  = get_unaligned_le16(&buf[7]);
++	gyro_pitch_minus = get_unaligned_le16(&buf[9]);
++	gyro_yaw_plus    = get_unaligned_le16(&buf[11]);
++	gyro_yaw_minus   = get_unaligned_le16(&buf[13]);
++	gyro_roll_plus   = get_unaligned_le16(&buf[15]);
++	gyro_roll_minus  = get_unaligned_le16(&buf[17]);
++	gyro_speed_plus  = get_unaligned_le16(&buf[19]);
++	gyro_speed_minus = get_unaligned_le16(&buf[21]);
++	acc_x_plus       = get_unaligned_le16(&buf[23]);
++	acc_x_minus      = get_unaligned_le16(&buf[25]);
++	acc_y_plus       = get_unaligned_le16(&buf[27]);
++	acc_y_minus      = get_unaligned_le16(&buf[29]);
++	acc_z_plus       = get_unaligned_le16(&buf[31]);
++	acc_z_minus      = get_unaligned_le16(&buf[33]);
++
++	/*
++	 * Set gyroscope calibration and normalization parameters.
++	 * Data values will be normalized to 1/DS_GYRO_RES_PER_DEG_S degree/s.
++	 */
++	speed_2x = (gyro_speed_plus + gyro_speed_minus);
++	ds->gyro_calib_data[0].abs_code = ABS_RX;
++	ds->gyro_calib_data[0].bias = gyro_pitch_bias;
++	ds->gyro_calib_data[0].sens_numer = speed_2x*DS_GYRO_RES_PER_DEG_S;
++	ds->gyro_calib_data[0].sens_denom = gyro_pitch_plus - gyro_pitch_minus;
++
++	ds->gyro_calib_data[1].abs_code = ABS_RY;
++	ds->gyro_calib_data[1].bias = gyro_yaw_bias;
++	ds->gyro_calib_data[1].sens_numer = speed_2x*DS_GYRO_RES_PER_DEG_S;
++	ds->gyro_calib_data[1].sens_denom = gyro_yaw_plus - gyro_yaw_minus;
++
++	ds->gyro_calib_data[2].abs_code = ABS_RZ;
++	ds->gyro_calib_data[2].bias = gyro_roll_bias;
++	ds->gyro_calib_data[2].sens_numer = speed_2x*DS_GYRO_RES_PER_DEG_S;
++	ds->gyro_calib_data[2].sens_denom = gyro_roll_plus - gyro_roll_minus;
++
++	/*
++	 * Set accelerometer calibration and normalization parameters.
++	 * Data values will be normalized to 1/DS_ACC_RES_PER_G g.
++	 */
++	range_2g = acc_x_plus - acc_x_minus;
++	ds->accel_calib_data[0].abs_code = ABS_X;
++	ds->accel_calib_data[0].bias = acc_x_plus - range_2g / 2;
++	ds->accel_calib_data[0].sens_numer = 2*DS_ACC_RES_PER_G;
++	ds->accel_calib_data[0].sens_denom = range_2g;
++
++	range_2g = acc_y_plus - acc_y_minus;
++	ds->accel_calib_data[1].abs_code = ABS_Y;
++	ds->accel_calib_data[1].bias = acc_y_plus - range_2g / 2;
++	ds->accel_calib_data[1].sens_numer = 2*DS_ACC_RES_PER_G;
++	ds->accel_calib_data[1].sens_denom = range_2g;
++
++	range_2g = acc_z_plus - acc_z_minus;
++	ds->accel_calib_data[2].abs_code = ABS_Z;
++	ds->accel_calib_data[2].bias = acc_z_plus - range_2g / 2;
++	ds->accel_calib_data[2].sens_numer = 2*DS_ACC_RES_PER_G;
++	ds->accel_calib_data[2].sens_denom = range_2g;
++
++err_free:
++	kfree(buf);
++	return ret;
++}
++
  static int dualsense_get_mac_address(struct dualsense *ds)
  {
  	uint8_t *buf;
-@@ -271,6 +304,7 @@ static int dualsense_parse_report(struct ps_device *ps_dev, struct hid_report *r
+@@ -303,7 +494,9 @@ static int dualsense_parse_report(struct ps_device *ps_dev, struct hid_report *r
+ 	struct dualsense_input_report *ds_report;
  	uint8_t battery_data, battery_capacity, charging_status, value;
  	int battery_status;
++	uint32_t sensor_timestamp;
  	unsigned long flags;
 +	int i;
  
- 	/* DualSense in USB uses the full HID report for reportID 1, but
- 	 * Bluetooth uses a minimal HID report for reportID 1 and reports
-@@ -311,6 +345,25 @@ static int dualsense_parse_report(struct ps_device *ps_dev, struct hid_report *r
- 	input_report_key(ds->gamepad, BTN_MODE, ds_report->buttons[2] & DS_BUTTONS2_PS_HOME);
+ 	/*
+ 	 * DualSense in USB uses the full HID report for reportID 1, but
+@@ -346,6 +539,63 @@ static int dualsense_parse_report(struct ps_device *ps_dev, struct hid_report *r
+ 	input_report_key(ds->gamepad, BTN_MODE,   ds_report->buttons[2] & DS_BUTTONS2_PS_HOME);
  	input_sync(ds->gamepad);
  
-+	input_report_key(ds->touchpad, BTN_LEFT, ds_report->buttons[2] & DS_BUTTONS2_TOUCHPAD);
-+	for (i = 0; i < 2; i++) {
-+		bool active = (ds_report->points[i].contact & 0x80) ? false : true;
++	/* Parse and calibrate gyroscope data. */
++	for (i = 0; i < ARRAY_SIZE(ds_report->gyro); i++) {
++		int raw_data = (short)le16_to_cpu(ds_report->gyro[i]);
++		int calib_data = mult_frac(ds->gyro_calib_data[i].sens_numer,
++					   raw_data - ds->gyro_calib_data[i].bias,
++					   ds->gyro_calib_data[i].sens_denom);
++
++		input_report_abs(ds->sensors, ds->gyro_calib_data[i].abs_code, calib_data);
++	}
++
++	/* Parse and calibrate accelerometer data. */
++	for (i = 0; i < ARRAY_SIZE(ds_report->accel); i++) {
++		int raw_data = (short)le16_to_cpu(ds_report->accel[i]);
++		int calib_data = mult_frac(ds->accel_calib_data[i].sens_numer,
++					   raw_data - ds->accel_calib_data[i].bias,
++					   ds->accel_calib_data[i].sens_denom);
++
++		input_report_abs(ds->sensors, ds->accel_calib_data[i].abs_code, calib_data);
++	}
++
++	/* Convert timestamp (in 0.33us unit) to timestamp_us */
++	sensor_timestamp = le32_to_cpu(ds_report->sensor_timestamp);
++	if (!ds->sensor_timestamp_initialized) {
++		ds->sensor_timestamp_us = DIV_ROUND_CLOSEST(sensor_timestamp, 3);
++		ds->sensor_timestamp_initialized = true;
++	} else {
++		uint32_t delta;
++
++		if (ds->prev_sensor_timestamp > sensor_timestamp)
++			delta = (U32_MAX - ds->prev_sensor_timestamp + sensor_timestamp + 1);
++		else
++			delta = sensor_timestamp - ds->prev_sensor_timestamp;
++		ds->sensor_timestamp_us += DIV_ROUND_CLOSEST(delta, 3);
++	}
++	ds->prev_sensor_timestamp = sensor_timestamp;
++	input_event(ds->sensors, EV_MSC, MSC_TIMESTAMP, ds->sensor_timestamp_us);
++	input_sync(ds->sensors);
++
++	for (i = 0; i < ARRAY_SIZE(ds_report->points); i++) {
++		struct dualsense_touch_point *point = &ds_report->points[i];
++		bool active = (point->contact & DS_TOUCH_POINT_INACTIVE) ? false : true;
 +
 +		input_mt_slot(ds->touchpad, i);
 +		input_mt_report_slot_state(ds->touchpad, MT_TOOL_FINGER, active);
 +
 +		if (active) {
-+			int x = (ds_report->points[i].x_hi << 8) | ds_report->points[i].x_lo;
-+			int y = (ds_report->points[i].y_hi << 4) | ds_report->points[i].y_lo;
++			int x = (point->x_hi << 8) | point->x_lo;
++			int y = (point->y_hi << 4) | point->y_lo;
 +
 +			input_report_abs(ds->touchpad, ABS_MT_POSITION_X, x);
 +			input_report_abs(ds->touchpad, ABS_MT_POSITION_Y, y);
@@ -95,18 +303,37 @@
  	battery_data = ds_report->status & DS_STATUS_BATTERY_CAPACITY;
  	charging_status = (ds_report->status & DS_STATUS_CHARGING) >> DS_STATUS_CHARGING_SHIFT;
  
-@@ -382,6 +435,12 @@ static struct ps_device *dualsense_create(struct hid_device *hdev)
+@@ -416,12 +666,31 @@ static struct ps_device *dualsense_create(struct hid_device *hdev)
+ 	}
+ 	snprintf(hdev->uniq, sizeof(hdev->uniq), "%pMR", ds->base.mac_address);
+ 
++	ret = dualsense_get_calibration_data(ds);
++	if (ret) {
++		hid_err(hdev, "Failed to get calibration data from DualSense\n");
++		goto err;
++	}
++
+ 	ds->gamepad = ps_gamepad_create(hdev);
+ 	if (IS_ERR(ds->gamepad)) {
+ 		ret = PTR_ERR(ds->gamepad);
  		goto err;
  	}
  
++	ds->sensors = ps_sensors_create(hdev, DS_ACC_RANGE, DS_ACC_RES_PER_G,
++					DS_GYRO_RANGE, DS_GYRO_RES_PER_DEG_S);
++	if (IS_ERR(ds->sensors)) {
++		ret = PTR_ERR(ds->sensors);
++		goto err;
++	}
++
 +	ds->touchpad = ps_touchpad_create(hdev, DS_TOUCHPAD_WIDTH, DS_TOUCHPAD_HEIGHT, 2);
 +	if (IS_ERR(ds->touchpad)) {
 +		ret = PTR_ERR(ds->touchpad);
 +		goto err;
 +	}
 +
- 	ret = ps_device_register_battery((struct ps_device *)ds);
- 	if (ret < 0)
+ 	ret = ps_device_register_battery(ps_dev);
+ 	if (ret)
  		goto err;
 -- 
 2.26.2