1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
26
27#include "stv06xx_st6422.h"
28
29static struct v4l2_pix_format st6422_mode[] = {
30
31
32 {
33 162,
34 120,
35 V4L2_PIX_FMT_SGRBG8,
36 V4L2_FIELD_NONE,
37 .sizeimage = 162 * 120,
38 .bytesperline = 162,
39 .colorspace = V4L2_COLORSPACE_SRGB,
40 .priv = 1
41 },
42
43
44
45
46 {
47 324,
48 240,
49 V4L2_PIX_FMT_SGRBG8,
50 V4L2_FIELD_NONE,
51 .sizeimage = 324 * 244,
52 .bytesperline = 324,
53 .colorspace = V4L2_COLORSPACE_SRGB,
54 .priv = 0
55 },
56};
57
58
59static int setbrightness(struct sd *sd, s32 val);
60static int setcontrast(struct sd *sd, s32 val);
61static int setgain(struct sd *sd, u8 gain);
62static int setexposure(struct sd *sd, s16 expo);
63
64static int st6422_s_ctrl(struct v4l2_ctrl *ctrl)
65{
66 struct gspca_dev *gspca_dev =
67 container_of(ctrl->handler, struct gspca_dev, ctrl_handler);
68 struct sd *sd = (struct sd *)gspca_dev;
69 int err = -EINVAL;
70
71 switch (ctrl->id) {
72 case V4L2_CID_BRIGHTNESS:
73 err = setbrightness(sd, ctrl->val);
74 break;
75 case V4L2_CID_CONTRAST:
76 err = setcontrast(sd, ctrl->val);
77 break;
78 case V4L2_CID_GAIN:
79 err = setgain(sd, ctrl->val);
80 break;
81 case V4L2_CID_EXPOSURE:
82 err = setexposure(sd, ctrl->val);
83 break;
84 }
85
86
87 if (err >= 0)
88 err = stv06xx_write_bridge(sd, 0x143f, 0x01);
89 sd->gspca_dev.usb_err = err;
90 return err;
91}
92
93static const struct v4l2_ctrl_ops st6422_ctrl_ops = {
94 .s_ctrl = st6422_s_ctrl,
95};
96
97static int st6422_init_controls(struct sd *sd)
98{
99 struct v4l2_ctrl_handler *hdl = &sd->gspca_dev.ctrl_handler;
100
101 v4l2_ctrl_handler_init(hdl, 4);
102 v4l2_ctrl_new_std(hdl, &st6422_ctrl_ops,
103 V4L2_CID_BRIGHTNESS, 0, 31, 1, 3);
104 v4l2_ctrl_new_std(hdl, &st6422_ctrl_ops,
105 V4L2_CID_CONTRAST, 0, 15, 1, 11);
106 v4l2_ctrl_new_std(hdl, &st6422_ctrl_ops,
107 V4L2_CID_EXPOSURE, 0, 1023, 1, 256);
108 v4l2_ctrl_new_std(hdl, &st6422_ctrl_ops,
109 V4L2_CID_GAIN, 0, 255, 1, 64);
110
111 return hdl->error;
112}
113
114static int st6422_probe(struct sd *sd)
115{
116 if (sd->bridge != BRIDGE_ST6422)
117 return -ENODEV;
118
119 pr_info("st6422 sensor detected\n");
120
121 sd->gspca_dev.cam.cam_mode = st6422_mode;
122 sd->gspca_dev.cam.nmodes = ARRAY_SIZE(st6422_mode);
123 return 0;
124}
125
126static int st6422_init(struct sd *sd)
127{
128 int err = 0, i;
129
130 const u16 st6422_bridge_init[][2] = {
131 { STV_ISO_ENABLE, 0x00 },
132 { 0x1436, 0x00 },
133 { 0x1432, 0x03 },
134 { 0x143a, 0xf9 },
135 { 0x0509, 0x38 },
136 { 0x050a, 0x38 },
137 { 0x050b, 0x38 },
138 { 0x050c, 0x2a },
139 { 0x050d, 0x01 },
140
141
142 { 0x1431, 0x00 },
143 { 0x1433, 0x34 },
144 { 0x1438, 0x18 },
145
146
147
148 { 0x1439, 0x00 },
149
150
151 { 0x143b, 0x05 },
152 { 0x143c, 0x00 },
153
154
155
156
157
158 { 0x143e, 0x01 },
159 { 0x143d, 0x00 },
160
161 { 0x1442, 0xe2 },
162
163
164
165
166
167
168
169
170
171
172
173
174
175 { 0x1500, 0xd0 },
176 { 0x1500, 0xd0 },
177 { 0x1500, 0x50 },
178
179 { 0x1501, 0xaf },
180
181
182 { 0x1502, 0xc2 },
183
184
185 { 0x1503, 0x45 },
186
187
188 { 0x1505, 0x02 },
189
190
191
192
193 { 0x150e, 0x8e },
194 { 0x150f, 0x37 },
195 { 0x15c0, 0x00 },
196 { 0x15c3, 0x08 },
197
198
199 { 0x143f, 0x01 },
200
201 };
202
203 for (i = 0; i < ARRAY_SIZE(st6422_bridge_init) && !err; i++) {
204 err = stv06xx_write_bridge(sd, st6422_bridge_init[i][0],
205 st6422_bridge_init[i][1]);
206 }
207
208 return err;
209}
210
211static int setbrightness(struct sd *sd, s32 val)
212{
213
214 return stv06xx_write_bridge(sd, 0x1432, val);
215}
216
217static int setcontrast(struct sd *sd, s32 val)
218{
219
220 return stv06xx_write_bridge(sd, 0x143a, val | 0xf0);
221}
222
223static int setgain(struct sd *sd, u8 gain)
224{
225 int err;
226
227
228 err = stv06xx_write_bridge(sd, 0x0509, gain);
229 if (err < 0)
230 return err;
231
232 err = stv06xx_write_bridge(sd, 0x050a, gain);
233 if (err < 0)
234 return err;
235
236 err = stv06xx_write_bridge(sd, 0x050b, gain);
237 if (err < 0)
238 return err;
239
240
241 err = stv06xx_write_bridge(sd, 0x050c, 0x2a);
242 if (err < 0)
243 return err;
244
245 return stv06xx_write_bridge(sd, 0x050d, 0x01);
246}
247
248static int setexposure(struct sd *sd, s16 expo)
249{
250 int err;
251
252 err = stv06xx_write_bridge(sd, 0x143d, expo & 0xff);
253 if (err < 0)
254 return err;
255
256 return stv06xx_write_bridge(sd, 0x143e, expo >> 8);
257}
258
259static int st6422_start(struct sd *sd)
260{
261 int err;
262 struct cam *cam = &sd->gspca_dev.cam;
263
264 if (cam->cam_mode[sd->gspca_dev.curr_mode].priv)
265 err = stv06xx_write_bridge(sd, 0x1505, 0x0f);
266 else
267 err = stv06xx_write_bridge(sd, 0x1505, 0x02);
268 if (err < 0)
269 return err;
270
271
272 err = stv06xx_write_bridge(sd, 0x143f, 0x01);
273 return (err < 0) ? err : 0;
274}
275
276static int st6422_stop(struct sd *sd)
277{
278 struct gspca_dev *gspca_dev = (struct gspca_dev *)sd;
279
280 gspca_dbg(gspca_dev, D_STREAM, "Halting stream\n");
281
282 return 0;
283}
284