3 files changed, 66 insertions, 1 deletions

diff --git a/drivers/media/video/pwc/pwc-ctrl.c b/drivers/media/video/pwc/pwc-ctrl.c
index 8e0cc537e1e4..d09413c44c86 100644
--- a/drivers/media/video/pwc/pwc-ctrl.c
+++ b/drivers/media/video/pwc/pwc-ctrl.c
@@ -165,7 +165,7 @@ static inline int send_video_command(struct pwc_device *pdev,
 		buf, buflen);
 }
 
-static inline int send_control_msg(struct pwc_device *pdev,
+int send_control_msg(struct pwc_device *pdev,
 	u8 request, u16 value, void *buf, int buflen)
 {
 	return _send_control_msg(pdev,
diff --git a/drivers/media/video/pwc/pwc-v4l.c b/drivers/media/video/pwc/pwc-v4l.c
index 537657283e79..e9a0e94b9995 100644
--- a/drivers/media/video/pwc/pwc-v4l.c
+++ b/drivers/media/video/pwc/pwc-v4l.c
@@ -338,6 +338,22 @@ int pwc_init_controls(struct pwc_device *pdev)
 	if (pdev->restore_factory)
 		pdev->restore_factory->flags = V4L2_CTRL_FLAG_UPDATE;
 
+	if (!pdev->features & FEATURE_MOTOR_PANTILT)
+		return hdl->error;
+
+	/* Motor pan / tilt / reset */
+	pdev->motor_pan = v4l2_ctrl_new_std(hdl, &pwc_ctrl_ops,
+				V4L2_CID_PAN_RELATIVE, -4480, 4480, 64, 0);
+	if (!pdev->motor_pan)
+		return hdl->error;
+	pdev->motor_tilt = v4l2_ctrl_new_std(hdl, &pwc_ctrl_ops,
+				V4L2_CID_TILT_RELATIVE, -1920, 1920, 64, 0);
+	pdev->motor_pan_reset = v4l2_ctrl_new_std(hdl, &pwc_ctrl_ops,
+				V4L2_CID_PAN_RESET, 0, 0, 0, 0);
+	pdev->motor_tilt_reset = v4l2_ctrl_new_std(hdl, &pwc_ctrl_ops,
+				V4L2_CID_TILT_RESET, 0, 0, 0, 0);
+	v4l2_ctrl_cluster(4, &pdev->motor_pan);
+
 	return hdl->error;
 }
 
@@ -764,6 +780,43 @@ static int pwc_set_autogain_expo(struct pwc_device *pdev)
 	return ret;
 }
 
+static int pwc_set_motor(struct pwc_device *pdev)
+{
+	int ret;
+	u8 buf[4];
+
+	buf[0] = 0;
+	if (pdev->motor_pan_reset->is_new)
+		buf[0] |= 0x01;
+	if (pdev->motor_tilt_reset->is_new)
+		buf[0] |= 0x02;
+	if (pdev->motor_pan_reset->is_new || pdev->motor_tilt_reset->is_new) {
+		ret = send_control_msg(pdev, SET_MPT_CTL,
+				       PT_RESET_CONTROL_FORMATTER, buf, 1);
+		if (ret < 0)
+			return ret;
+	}
+
+	memset(buf, 0, sizeof(buf));
+	if (pdev->motor_pan->is_new) {
+		buf[0] = pdev->motor_pan->val & 0xFF;
+		buf[1] = (pdev->motor_pan->val >> 8);
+	}
+	if (pdev->motor_tilt->is_new) {
+		buf[2] = pdev->motor_tilt->val & 0xFF;
+		buf[3] = (pdev->motor_tilt->val >> 8);
+	}
+	if (pdev->motor_pan->is_new || pdev->motor_tilt->is_new) {
+		ret = send_control_msg(pdev, SET_MPT_CTL,
+				       PT_RELATIVE_CONTROL_FORMATTER,
+				       buf, sizeof(buf));
+		if (ret < 0)
+			return ret;
+	}
+
+	return 0;
+}
+
 static int pwc_s_ctrl(struct v4l2_ctrl *ctrl)
 {
 	struct pwc_device *pdev =
@@ -859,6 +912,9 @@ static int pwc_s_ctrl(struct v4l2_ctrl *ctrl)
 		ret = pwc_button_ctrl(pdev,
 				      RESTORE_FACTORY_DEFAULTS_FORMATTER);
 		break;
+	case V4L2_CID_PAN_RELATIVE:
+		ret = pwc_set_motor(pdev);
+		break;
 	default:
 		ret = -EINVAL;
 	}
diff --git a/drivers/media/video/pwc/pwc.h b/drivers/media/video/pwc/pwc.h
index e02dbf745155..8f3607be5a71 100644
--- a/drivers/media/video/pwc/pwc.h
+++ b/drivers/media/video/pwc/pwc.h
@@ -326,6 +326,13 @@ struct pwc_device
 	struct v4l2_ctrl		*save_user;
 	struct v4l2_ctrl		*restore_user;
 	struct v4l2_ctrl		*restore_factory;
+	struct {
+		/* motor control cluster */
+		struct v4l2_ctrl	*motor_pan;
+		struct v4l2_ctrl	*motor_tilt;
+		struct v4l2_ctrl	*motor_pan_reset;
+		struct v4l2_ctrl	*motor_tilt_reset;
+	};
 	/* CODEC3 models have both gain and exposure controlled by autogain */
 	struct v4l2_ctrl		*autogain_expo_cluster[3];
 };
@@ -350,6 +357,8 @@ extern int pwc_mpt_reset(struct pwc_device *pdev, int flags);
 extern int pwc_mpt_set_angle(struct pwc_device *pdev, int pan, int tilt);
 extern int pwc_set_leds(struct pwc_device *pdev, int on_value, int off_value);
 extern int pwc_get_cmos_sensor(struct pwc_device *pdev, int *sensor);
+extern int send_control_msg(struct pwc_device *pdev,
+			    u8 request, u16 value, void *buf, int buflen);
 
 /* Control get / set helpers */
 int pwc_get_u8_ctrl(struct pwc_device *pdev, u8 request, u16 value, int *data);