Pull throttle into release branch
[linux-2.6] / drivers / media / video / cpia2 / cpia2_core.c
1 /****************************************************************************
2  *
3  *  Filename: cpia2_core.c
4  *
5  *  Copyright 2001, STMicrolectronics, Inc.
6  *      Contact:  steve.miller@st.com
7  *
8  *  Description:
9  *     This is a USB driver for CPia2 based video cameras.
10  *     The infrastructure of this driver is based on the cpia usb driver by
11  *     Jochen Scharrlach and Johannes Erdfeldt.
12  *
13  *  This program is free software; you can redistribute it and/or modify
14  *  it under the terms of the GNU General Public License as published by
15  *  the Free Software Foundation; either version 2 of the License, or
16  *  (at your option) any later version.
17  *
18  *  This program is distributed in the hope that it will be useful,
19  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
20  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
21  *  GNU General Public License for more details.
22  *
23  *  You should have received a copy of the GNU General Public License
24  *  along with this program; if not, write to the Free Software
25  *  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
26  *
27  *  Stripped of 2.4 stuff ready for main kernel submit by
28  *              Alan Cox <alan@redhat.com>
29  *
30  ****************************************************************************/
31
32 #include "cpia2.h"
33
34 #include <linux/slab.h>
35 #include <linux/vmalloc.h>
36
37 //#define _CPIA2_DEBUG_
38
39 #include "cpia2patch.h"
40
41 #ifdef _CPIA2_DEBUG_
42
43 static const char *block_name[] = {
44         "System",
45         "VC",
46         "VP",
47         "IDATA"
48 };
49 #endif
50
51 static unsigned int debugs_on = 0;//DEBUG_REG;
52
53
54 /******************************************************************************
55  *
56  *  Forward Declarations
57  *
58  *****************************************************************************/
59 static int apply_vp_patch(struct camera_data *cam);
60 static int set_default_user_mode(struct camera_data *cam);
61 static int set_vw_size(struct camera_data *cam, int size);
62 static int configure_sensor(struct camera_data *cam,
63                             int reqwidth, int reqheight);
64 static int config_sensor_410(struct camera_data *cam,
65                             int reqwidth, int reqheight);
66 static int config_sensor_500(struct camera_data *cam,
67                             int reqwidth, int reqheight);
68 static int set_all_properties(struct camera_data *cam);
69 static void get_color_params(struct camera_data *cam);
70 static void wake_system(struct camera_data *cam);
71 static void set_lowlight_boost(struct camera_data *cam);
72 static void reset_camera_struct(struct camera_data *cam);
73 static int cpia2_set_high_power(struct camera_data *cam);
74
75 /* Here we want the physical address of the memory.
76  * This is used when initializing the contents of the
77  * area and marking the pages as reserved.
78  */
79 static inline unsigned long kvirt_to_pa(unsigned long adr)
80 {
81         unsigned long kva, ret;
82
83         kva = (unsigned long) page_address(vmalloc_to_page((void *)adr));
84         kva |= adr & (PAGE_SIZE-1); /* restore the offset */
85         ret = __pa(kva);
86         return ret;
87 }
88
89 static void *rvmalloc(unsigned long size)
90 {
91         void *mem;
92         unsigned long adr;
93
94         /* Round it off to PAGE_SIZE */
95         size = PAGE_ALIGN(size);
96
97         mem = vmalloc_32(size);
98         if (!mem)
99                 return NULL;
100
101         memset(mem, 0, size);   /* Clear the ram out, no junk to the user */
102         adr = (unsigned long) mem;
103
104         while ((long)size > 0) {
105                 SetPageReserved(vmalloc_to_page((void *)adr));
106                 adr += PAGE_SIZE;
107                 size -= PAGE_SIZE;
108         }
109         return mem;
110 }
111
112 static void rvfree(void *mem, unsigned long size)
113 {
114         unsigned long adr;
115
116         if (!mem)
117                 return;
118
119         size = PAGE_ALIGN(size);
120
121         adr = (unsigned long) mem;
122         while ((long)size > 0) {
123                 ClearPageReserved(vmalloc_to_page((void *)adr));
124                 adr += PAGE_SIZE;
125                 size -= PAGE_SIZE;
126         }
127         vfree(mem);
128 }
129
130 /******************************************************************************
131  *
132  *  cpia2_do_command
133  *
134  *  Send an arbitrary command to the camera.  For commands that read from
135  *  the camera, copy the buffers into the proper param structures.
136  *****************************************************************************/
137 int cpia2_do_command(struct camera_data *cam,
138                      u32 command, u8 direction, u8 param)
139 {
140         int retval = 0;
141         struct cpia2_command cmd;
142         unsigned int device = cam->params.pnp_id.device_type;
143
144         cmd.command = command;
145         cmd.reg_count = 2;      /* default */
146         cmd.direction = direction;
147
148         /***
149          * Set up the command.
150          ***/
151         switch (command) {
152         case CPIA2_CMD_GET_VERSION:
153                 cmd.req_mode =
154                     CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_SYSTEM;
155                 cmd.start = CPIA2_SYSTEM_DEVICE_HI;
156                 break;
157         case CPIA2_CMD_GET_PNP_ID:
158                 cmd.req_mode =
159                     CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_SYSTEM;
160                 cmd.reg_count = 8;
161                 cmd.start = CPIA2_SYSTEM_DESCRIP_VID_HI;
162                 break;
163         case CPIA2_CMD_GET_ASIC_TYPE:
164                 cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VC;
165                 cmd.start = CPIA2_VC_ASIC_ID;
166                 break;
167         case CPIA2_CMD_GET_SENSOR:
168                 cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
169                 cmd.start = CPIA2_VP_SENSOR_FLAGS;
170                 break;
171         case CPIA2_CMD_GET_VP_DEVICE:
172                 cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
173                 cmd.start = CPIA2_VP_DEVICEH;
174                 break;
175         case CPIA2_CMD_SET_VP_BRIGHTNESS:
176                 cmd.buffer.block_data[0] = param;       /* Then fall through */
177         case CPIA2_CMD_GET_VP_BRIGHTNESS:
178                 cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
179                 cmd.reg_count = 1;
180                 if (device == DEVICE_STV_672)
181                         cmd.start = CPIA2_VP4_EXPOSURE_TARGET;
182                 else
183                         cmd.start = CPIA2_VP5_EXPOSURE_TARGET;
184                 break;
185         case CPIA2_CMD_SET_CONTRAST:
186                 cmd.buffer.block_data[0] = param;       /* Then fall through */
187         case CPIA2_CMD_GET_CONTRAST:
188                 cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
189                 cmd.reg_count = 1;
190                 cmd.start = CPIA2_VP_YRANGE;
191                 break;
192         case CPIA2_CMD_SET_VP_SATURATION:
193                 cmd.buffer.block_data[0] = param;       /* Then fall through */
194         case CPIA2_CMD_GET_VP_SATURATION:
195                 cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
196                 cmd.reg_count = 1;
197                 if (device == DEVICE_STV_672)
198                         cmd.start = CPIA2_VP_SATURATION;
199                 else
200                         cmd.start = CPIA2_VP5_MCUVSATURATION;
201                 break;
202         case CPIA2_CMD_SET_VP_GPIO_DATA:
203                 cmd.buffer.block_data[0] = param;       /* Then fall through */
204         case CPIA2_CMD_GET_VP_GPIO_DATA:
205                 cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
206                 cmd.reg_count = 1;
207                 cmd.start = CPIA2_VP_GPIO_DATA;
208                 break;
209         case CPIA2_CMD_SET_VP_GPIO_DIRECTION:
210                 cmd.buffer.block_data[0] = param;       /* Then fall through */
211         case CPIA2_CMD_GET_VP_GPIO_DIRECTION:
212                 cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
213                 cmd.reg_count = 1;
214                 cmd.start = CPIA2_VP_GPIO_DIRECTION;
215                 break;
216         case CPIA2_CMD_SET_VC_MP_GPIO_DATA:
217                 cmd.buffer.block_data[0] = param;       /* Then fall through */
218         case CPIA2_CMD_GET_VC_MP_GPIO_DATA:
219                 cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VC;
220                 cmd.reg_count = 1;
221                 cmd.start = CPIA2_VC_MP_DATA;
222                 break;
223         case CPIA2_CMD_SET_VC_MP_GPIO_DIRECTION:
224                 cmd.buffer.block_data[0] = param;       /* Then fall through */
225         case CPIA2_CMD_GET_VC_MP_GPIO_DIRECTION:
226                 cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VC;
227                 cmd.reg_count = 1;
228                 cmd.start = CPIA2_VC_MP_DIR;
229                 break;
230         case CPIA2_CMD_ENABLE_PACKET_CTRL:
231                 cmd.req_mode =
232                     CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_SYSTEM;
233                 cmd.start = CPIA2_SYSTEM_INT_PACKET_CTRL;
234                 cmd.reg_count = 1;
235                 cmd.buffer.block_data[0] = param;
236                 break;
237         case CPIA2_CMD_SET_FLICKER_MODES:
238                 cmd.buffer.block_data[0] = param;       /* Then fall through */
239         case CPIA2_CMD_GET_FLICKER_MODES:
240                 cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
241                 cmd.reg_count = 1;
242                 cmd.start = CPIA2_VP_FLICKER_MODES;
243                 break;
244         case CPIA2_CMD_RESET_FIFO:      /* clear fifo and enable stream block */
245                 cmd.req_mode = CAMERAACCESS_TYPE_RANDOM | CAMERAACCESS_VC;
246                 cmd.reg_count = 2;
247                 cmd.start = 0;
248                 cmd.buffer.registers[0].index = CPIA2_VC_ST_CTRL;
249                 cmd.buffer.registers[0].value = CPIA2_VC_ST_CTRL_SRC_VC |
250                     CPIA2_VC_ST_CTRL_DST_USB | CPIA2_VC_ST_CTRL_EOF_DETECT;
251                 cmd.buffer.registers[1].index = CPIA2_VC_ST_CTRL;
252                 cmd.buffer.registers[1].value = CPIA2_VC_ST_CTRL_SRC_VC |
253                     CPIA2_VC_ST_CTRL_DST_USB |
254                     CPIA2_VC_ST_CTRL_EOF_DETECT |
255                     CPIA2_VC_ST_CTRL_FIFO_ENABLE;
256                 break;
257         case CPIA2_CMD_SET_HI_POWER:
258                 cmd.req_mode =
259                     CAMERAACCESS_TYPE_RANDOM | CAMERAACCESS_SYSTEM;
260                 cmd.reg_count = 2;
261                 cmd.buffer.registers[0].index =
262                     CPIA2_SYSTEM_SYSTEM_CONTROL;
263                 cmd.buffer.registers[1].index =
264                     CPIA2_SYSTEM_SYSTEM_CONTROL;
265                 cmd.buffer.registers[0].value = CPIA2_SYSTEM_CONTROL_CLEAR_ERR;
266                 cmd.buffer.registers[1].value =
267                     CPIA2_SYSTEM_CONTROL_HIGH_POWER;
268                 break;
269         case CPIA2_CMD_SET_LOW_POWER:
270                 cmd.req_mode =
271                     CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_SYSTEM;
272                 cmd.reg_count = 1;
273                 cmd.start = CPIA2_SYSTEM_SYSTEM_CONTROL;
274                 cmd.buffer.block_data[0] = 0;
275                 break;
276         case CPIA2_CMD_CLEAR_V2W_ERR:
277                 cmd.req_mode =
278                     CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_SYSTEM;
279                 cmd.reg_count = 1;
280                 cmd.start = CPIA2_SYSTEM_SYSTEM_CONTROL;
281                 cmd.buffer.block_data[0] = CPIA2_SYSTEM_CONTROL_CLEAR_ERR;
282                 break;
283         case CPIA2_CMD_SET_USER_MODE:   /* Then fall through */
284                 cmd.buffer.block_data[0] = param;
285         case CPIA2_CMD_GET_USER_MODE:
286                 cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
287                 cmd.reg_count = 1;
288                 if (device == DEVICE_STV_672)
289                         cmd.start = CPIA2_VP4_USER_MODE;
290                 else
291                         cmd.start = CPIA2_VP5_USER_MODE;
292                 break;
293         case CPIA2_CMD_FRAMERATE_REQ:
294                 cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
295                 cmd.reg_count = 1;
296                 if (device == DEVICE_STV_672)
297                         cmd.start = CPIA2_VP4_FRAMERATE_REQUEST;
298                 else
299                         cmd.start = CPIA2_VP5_FRAMERATE_REQUEST;
300                 cmd.buffer.block_data[0] = param;
301                 break;
302         case CPIA2_CMD_SET_WAKEUP:
303                 cmd.buffer.block_data[0] = param;       /* Then fall through */
304         case CPIA2_CMD_GET_WAKEUP:
305                 cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VC;
306                 cmd.reg_count = 1;
307                 cmd.start = CPIA2_VC_WAKEUP;
308                 break;
309         case CPIA2_CMD_SET_PW_CONTROL:
310                 cmd.buffer.block_data[0] = param;       /* Then fall through */
311         case CPIA2_CMD_GET_PW_CONTROL:
312                 cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VC;
313                 cmd.reg_count = 1;
314                 cmd.start = CPIA2_VC_PW_CTRL;
315                 break;
316         case CPIA2_CMD_GET_VP_SYSTEM_STATE:
317                 cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
318                 cmd.reg_count = 1;
319                 cmd.start = CPIA2_VP_SYSTEMSTATE;
320                 break;
321         case CPIA2_CMD_SET_SYSTEM_CTRL:
322                 cmd.buffer.block_data[0] = param;       /* Then fall through */
323         case CPIA2_CMD_GET_SYSTEM_CTRL:
324                 cmd.req_mode =
325                     CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_SYSTEM;
326                 cmd.reg_count = 1;
327                 cmd.start = CPIA2_SYSTEM_SYSTEM_CONTROL;
328                 break;
329         case CPIA2_CMD_SET_VP_SYSTEM_CTRL:
330                 cmd.buffer.block_data[0] = param;       /* Then fall through */
331         case CPIA2_CMD_GET_VP_SYSTEM_CTRL:
332                 cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
333                 cmd.reg_count = 1;
334                 cmd.start = CPIA2_VP_SYSTEMCTRL;
335                 break;
336         case CPIA2_CMD_SET_VP_EXP_MODES:
337                 cmd.buffer.block_data[0] = param;       /* Then fall through */
338         case CPIA2_CMD_GET_VP_EXP_MODES:
339                 cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
340                 cmd.reg_count = 1;
341                 cmd.start = CPIA2_VP_EXPOSURE_MODES;
342                 break;
343         case CPIA2_CMD_SET_DEVICE_CONFIG:
344                 cmd.buffer.block_data[0] = param;       /* Then fall through */
345         case CPIA2_CMD_GET_DEVICE_CONFIG:
346                 cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
347                 cmd.reg_count = 1;
348                 cmd.start = CPIA2_VP_DEVICE_CONFIG;
349                 break;
350         case CPIA2_CMD_SET_SERIAL_ADDR:
351                 cmd.buffer.block_data[0] = param;
352                 cmd.req_mode =
353                     CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_SYSTEM;
354                 cmd.reg_count = 1;
355                 cmd.start = CPIA2_SYSTEM_VP_SERIAL_ADDR;
356                 break;
357         case CPIA2_CMD_SET_SENSOR_CR1:
358                 cmd.buffer.block_data[0] = param;
359                 cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
360                 cmd.reg_count = 1;
361                 cmd.start = CPIA2_SENSOR_CR1;
362                 break;
363         case CPIA2_CMD_SET_VC_CONTROL:
364                 cmd.buffer.block_data[0] = param;       /* Then fall through */
365         case CPIA2_CMD_GET_VC_CONTROL:
366                 cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VC;
367                 cmd.reg_count = 1;
368                 cmd.start = CPIA2_VC_VC_CTRL;
369                 break;
370         case CPIA2_CMD_SET_TARGET_KB:
371                 cmd.req_mode = CAMERAACCESS_TYPE_RANDOM | CAMERAACCESS_VC;
372                 cmd.reg_count = 1;
373                 cmd.buffer.registers[0].index = CPIA2_VC_VC_TARGET_KB;
374                 cmd.buffer.registers[0].value = param;
375                 break;
376         case CPIA2_CMD_SET_DEF_JPEG_OPT:
377                 cmd.req_mode = CAMERAACCESS_TYPE_RANDOM | CAMERAACCESS_VC;
378                 cmd.reg_count = 4;
379                 cmd.buffer.registers[0].index = CPIA2_VC_VC_JPEG_OPT;
380                 cmd.buffer.registers[0].value =
381                     CPIA2_VC_VC_JPEG_OPT_DOUBLE_SQUEEZE;
382                 cmd.buffer.registers[1].index = CPIA2_VC_VC_USER_SQUEEZE;
383                 cmd.buffer.registers[1].value = 20;
384                 cmd.buffer.registers[2].index = CPIA2_VC_VC_CREEP_PERIOD;
385                 cmd.buffer.registers[2].value = 2;
386                 cmd.buffer.registers[3].index = CPIA2_VC_VC_JPEG_OPT;
387                 cmd.buffer.registers[3].value = CPIA2_VC_VC_JPEG_OPT_DEFAULT;
388                 break;
389         case CPIA2_CMD_REHASH_VP4:
390                 cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
391                 cmd.reg_count = 1;
392                 cmd.start = CPIA2_VP_REHASH_VALUES;
393                 cmd.buffer.block_data[0] = param;
394                 break;
395         case CPIA2_CMD_SET_USER_EFFECTS:  /* Note: Be careful with this as
396                                              this register can also affect
397                                              flicker modes */
398                 cmd.buffer.block_data[0] = param;      /* Then fall through */
399         case CPIA2_CMD_GET_USER_EFFECTS:
400                 cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
401                 cmd.reg_count = 1;
402                 if (device == DEVICE_STV_672)
403                         cmd.start = CPIA2_VP4_USER_EFFECTS;
404                 else
405                         cmd.start = CPIA2_VP5_USER_EFFECTS;
406                 break;
407         default:
408                 LOG("DoCommand received invalid command\n");
409                 return -EINVAL;
410         }
411
412         retval = cpia2_send_command(cam, &cmd);
413         if (retval) {
414                 return retval;
415         }
416
417         /***
418          * Now copy any results from a read into the appropriate param struct.
419          ***/
420         switch (command) {
421         case CPIA2_CMD_GET_VERSION:
422                 cam->params.version.firmware_revision_hi =
423                     cmd.buffer.block_data[0];
424                 cam->params.version.firmware_revision_lo =
425                     cmd.buffer.block_data[1];
426                 break;
427         case CPIA2_CMD_GET_PNP_ID:
428                 cam->params.pnp_id.vendor = (cmd.buffer.block_data[0] << 8) |
429                                             cmd.buffer.block_data[1];
430                 cam->params.pnp_id.product = (cmd.buffer.block_data[2] << 8) |
431                                              cmd.buffer.block_data[3];
432                 cam->params.pnp_id.device_revision =
433                         (cmd.buffer.block_data[4] << 8) |
434                         cmd.buffer.block_data[5];
435                 if (cam->params.pnp_id.vendor == 0x553) {
436                         if (cam->params.pnp_id.product == 0x100) {
437                                 cam->params.pnp_id.device_type = DEVICE_STV_672;
438                         } else if (cam->params.pnp_id.product == 0x140 ||
439                                    cam->params.pnp_id.product == 0x151) {
440                                 cam->params.pnp_id.device_type = DEVICE_STV_676;
441                         }
442                 }
443                 break;
444         case CPIA2_CMD_GET_ASIC_TYPE:
445                 cam->params.version.asic_id = cmd.buffer.block_data[0];
446                 cam->params.version.asic_rev = cmd.buffer.block_data[1];
447                 break;
448         case CPIA2_CMD_GET_SENSOR:
449                 cam->params.version.sensor_flags = cmd.buffer.block_data[0];
450                 cam->params.version.sensor_rev = cmd.buffer.block_data[1];
451                 break;
452         case CPIA2_CMD_GET_VP_DEVICE:
453                 cam->params.version.vp_device_hi = cmd.buffer.block_data[0];
454                 cam->params.version.vp_device_lo = cmd.buffer.block_data[1];
455                 break;
456         case CPIA2_CMD_GET_VP_BRIGHTNESS:
457                 cam->params.color_params.brightness = cmd.buffer.block_data[0];
458                 break;
459         case CPIA2_CMD_GET_CONTRAST:
460                 cam->params.color_params.contrast = cmd.buffer.block_data[0];
461                 break;
462         case CPIA2_CMD_GET_VP_SATURATION:
463                 cam->params.color_params.saturation = cmd.buffer.block_data[0];
464                 break;
465         case CPIA2_CMD_GET_VP_GPIO_DATA:
466                 cam->params.vp_params.gpio_data = cmd.buffer.block_data[0];
467                 break;
468         case CPIA2_CMD_GET_VP_GPIO_DIRECTION:
469                 cam->params.vp_params.gpio_direction = cmd.buffer.block_data[0];
470                 break;
471         case CPIA2_CMD_GET_VC_MP_GPIO_DIRECTION:
472                 cam->params.vc_params.vc_mp_direction =cmd.buffer.block_data[0];
473                 break;
474         case CPIA2_CMD_GET_VC_MP_GPIO_DATA:
475                 cam->params.vc_params.vc_mp_data = cmd.buffer.block_data[0];
476                 break;
477         case CPIA2_CMD_GET_FLICKER_MODES:
478                 cam->params.flicker_control.cam_register =
479                         cmd.buffer.block_data[0];
480                 break;
481         case CPIA2_CMD_GET_WAKEUP:
482                 cam->params.vc_params.wakeup = cmd.buffer.block_data[0];
483                 break;
484         case CPIA2_CMD_GET_PW_CONTROL:
485                 cam->params.vc_params.pw_control = cmd.buffer.block_data[0];
486                 break;
487         case CPIA2_CMD_GET_SYSTEM_CTRL:
488                 cam->params.camera_state.system_ctrl = cmd.buffer.block_data[0];
489                 break;
490         case CPIA2_CMD_GET_VP_SYSTEM_STATE:
491                 cam->params.vp_params.system_state = cmd.buffer.block_data[0];
492                 break;
493         case CPIA2_CMD_GET_VP_SYSTEM_CTRL:
494                 cam->params.vp_params.system_ctrl = cmd.buffer.block_data[0];
495                 break;
496         case CPIA2_CMD_GET_VP_EXP_MODES:
497                 cam->params.vp_params.exposure_modes = cmd.buffer.block_data[0];
498                 break;
499         case CPIA2_CMD_GET_DEVICE_CONFIG:
500                 cam->params.vp_params.device_config = cmd.buffer.block_data[0];
501                 break;
502         case CPIA2_CMD_GET_VC_CONTROL:
503                 cam->params.vc_params.vc_control = cmd.buffer.block_data[0];
504                 break;
505         case CPIA2_CMD_GET_USER_MODE:
506                 cam->params.vp_params.video_mode = cmd.buffer.block_data[0];
507                 break;
508         case CPIA2_CMD_GET_USER_EFFECTS:
509                 cam->params.vp_params.user_effects = cmd.buffer.block_data[0];
510                 break;
511         default:
512                 break;
513         }
514         return retval;
515 }
516
517 /******************************************************************************
518  *
519  *  cpia2_send_command
520  *
521  *****************************************************************************/
522 int cpia2_send_command(struct camera_data *cam, struct cpia2_command *cmd)
523 {
524         u8 count;
525         u8 start;
526         u8 block_index;
527         u8 *buffer;
528         int retval;
529         const char* dir;
530
531         if (cmd->direction == TRANSFER_WRITE) {
532                 dir = "Write";
533         } else {
534                 dir = "Read";
535         }
536
537         block_index = cmd->req_mode & 0x03;
538
539         switch (cmd->req_mode & 0x0c) {
540         case CAMERAACCESS_TYPE_RANDOM:
541                 count = cmd->reg_count * sizeof(struct cpia2_register);
542                 start = 0;
543                 buffer = (u8 *) & cmd->buffer;
544                 if (debugs_on & DEBUG_REG)
545                         DBG("%s Random: Register block %s\n", dir,
546                             block_name[block_index]);
547                 break;
548         case CAMERAACCESS_TYPE_BLOCK:
549                 count = cmd->reg_count;
550                 start = cmd->start;
551                 buffer = cmd->buffer.block_data;
552                 if (debugs_on & DEBUG_REG)
553                         DBG("%s Block: Register block %s\n", dir,
554                             block_name[block_index]);
555                 break;
556         case CAMERAACCESS_TYPE_MASK:
557                 count = cmd->reg_count * sizeof(struct cpia2_reg_mask);
558                 start = 0;
559                 buffer = (u8 *) & cmd->buffer;
560                 if (debugs_on & DEBUG_REG)
561                         DBG("%s Mask: Register block %s\n", dir,
562                             block_name[block_index]);
563                 break;
564         case CAMERAACCESS_TYPE_REPEAT:  /* For patch blocks only */
565                 count = cmd->reg_count;
566                 start = cmd->start;
567                 buffer = cmd->buffer.block_data;
568                 if (debugs_on & DEBUG_REG)
569                         DBG("%s Repeat: Register block %s\n", dir,
570                             block_name[block_index]);
571                 break;
572         default:
573                 LOG("%s: invalid request mode\n",__FUNCTION__);
574                 return -EINVAL;
575         }
576
577         retval = cpia2_usb_transfer_cmd(cam,
578                                         buffer,
579                                         cmd->req_mode,
580                                         start, count, cmd->direction);
581 #ifdef _CPIA2_DEBUG_
582         if (debugs_on & DEBUG_REG) {
583                 int i;
584                 for (i = 0; i < cmd->reg_count; i++) {
585                         if((cmd->req_mode & 0x0c) == CAMERAACCESS_TYPE_BLOCK)
586                                 KINFO("%s Block: [0x%02X] = 0x%02X\n",
587                                     dir, start + i, buffer[i]);
588                         if((cmd->req_mode & 0x0c) == CAMERAACCESS_TYPE_RANDOM)
589                                 KINFO("%s Random: [0x%02X] = 0x%02X\n",
590                                     dir, cmd->buffer.registers[i].index,
591                                     cmd->buffer.registers[i].value);
592                 }
593         }
594 #endif
595
596         return retval;
597 };
598
599 /*************
600  * Functions to implement camera functionality
601  *************/
602 /******************************************************************************
603  *
604  *  cpia2_get_version_info
605  *
606  *****************************************************************************/
607 static void cpia2_get_version_info(struct camera_data *cam)
608 {
609         cpia2_do_command(cam, CPIA2_CMD_GET_VERSION, TRANSFER_READ, 0);
610         cpia2_do_command(cam, CPIA2_CMD_GET_PNP_ID, TRANSFER_READ, 0);
611         cpia2_do_command(cam, CPIA2_CMD_GET_ASIC_TYPE, TRANSFER_READ, 0);
612         cpia2_do_command(cam, CPIA2_CMD_GET_SENSOR, TRANSFER_READ, 0);
613         cpia2_do_command(cam, CPIA2_CMD_GET_VP_DEVICE, TRANSFER_READ, 0);
614 }
615
616 /******************************************************************************
617  *
618  *  cpia2_reset_camera
619  *
620  *  Called at least during the open process, sets up initial params.
621  *****************************************************************************/
622 int cpia2_reset_camera(struct camera_data *cam)
623 {
624         u8 tmp_reg;
625         int retval = 0;
626         int i;
627         struct cpia2_command cmd;
628
629         /***
630          * VC setup
631          ***/
632         retval = configure_sensor(cam,
633                                   cam->params.roi.width,
634                                   cam->params.roi.height);
635         if (retval < 0) {
636                 ERR("Couldn't configure sensor, error=%d\n", retval);
637                 return retval;
638         }
639
640         /* Clear FIFO and route/enable stream block */
641         cmd.req_mode = CAMERAACCESS_TYPE_RANDOM | CAMERAACCESS_VC;
642         cmd.direction = TRANSFER_WRITE;
643         cmd.reg_count = 2;
644         cmd.buffer.registers[0].index = CPIA2_VC_ST_CTRL;
645         cmd.buffer.registers[0].value = CPIA2_VC_ST_CTRL_SRC_VC |
646                 CPIA2_VC_ST_CTRL_DST_USB | CPIA2_VC_ST_CTRL_EOF_DETECT;
647         cmd.buffer.registers[1].index = CPIA2_VC_ST_CTRL;
648         cmd.buffer.registers[1].value = CPIA2_VC_ST_CTRL_SRC_VC |
649                 CPIA2_VC_ST_CTRL_DST_USB |
650                 CPIA2_VC_ST_CTRL_EOF_DETECT | CPIA2_VC_ST_CTRL_FIFO_ENABLE;
651
652         cpia2_send_command(cam, &cmd);
653
654         cpia2_set_high_power(cam);
655
656         if (cam->params.pnp_id.device_type == DEVICE_STV_672) {
657                 /* Enable button notification */
658                 cmd.req_mode = CAMERAACCESS_TYPE_RANDOM | CAMERAACCESS_SYSTEM;
659                 cmd.buffer.registers[0].index = CPIA2_SYSTEM_INT_PACKET_CTRL;
660                 cmd.buffer.registers[0].value =
661                         CPIA2_SYSTEM_INT_PACKET_CTRL_ENABLE_SW_XX;
662                 cmd.reg_count = 1;
663                 cpia2_send_command(cam, &cmd);
664         }
665
666         schedule_timeout_interruptible(msecs_to_jiffies(100));
667
668         if (cam->params.pnp_id.device_type == DEVICE_STV_672)
669                 retval = apply_vp_patch(cam);
670
671         /* wait for vp to go to sleep */
672         schedule_timeout_interruptible(msecs_to_jiffies(100));
673
674         /***
675          * If this is a 676, apply VP5 fixes before we start streaming
676          ***/
677         if (cam->params.pnp_id.device_type == DEVICE_STV_676) {
678                 cmd.req_mode = CAMERAACCESS_TYPE_RANDOM | CAMERAACCESS_VP;
679
680                 /* The following writes improve the picture */
681                 cmd.buffer.registers[0].index = CPIA2_VP5_MYBLACK_LEVEL;
682                 cmd.buffer.registers[0].value = 0; /* reduce from the default
683                                                     * rec 601 pedestal of 16 */
684                 cmd.buffer.registers[1].index = CPIA2_VP5_MCYRANGE;
685                 cmd.buffer.registers[1].value = 0x92; /* increase from 100% to
686                                                        * (256/256 - 31) to fill
687                                                        * available range */
688                 cmd.buffer.registers[2].index = CPIA2_VP5_MYCEILING;
689                 cmd.buffer.registers[2].value = 0xFF; /* Increase from the
690                                                        * default rec 601 ceiling
691                                                        * of 240 */
692                 cmd.buffer.registers[3].index = CPIA2_VP5_MCUVSATURATION;
693                 cmd.buffer.registers[3].value = 0xFF; /* Increase from the rec
694                                                        * 601 100% level (128)
695                                                        * to 145-192 */
696                 cmd.buffer.registers[4].index = CPIA2_VP5_ANTIFLKRSETUP;
697                 cmd.buffer.registers[4].value = 0x80;  /* Inhibit the
698                                                         * anti-flicker */
699
700                 /* The following 4 writes are a fix to allow QVGA to work at 30 fps */
701                 cmd.buffer.registers[5].index = CPIA2_VP_RAM_ADDR_H;
702                 cmd.buffer.registers[5].value = 0x01;
703                 cmd.buffer.registers[6].index = CPIA2_VP_RAM_ADDR_L;
704                 cmd.buffer.registers[6].value = 0xE3;
705                 cmd.buffer.registers[7].index = CPIA2_VP_RAM_DATA;
706                 cmd.buffer.registers[7].value = 0x02;
707                 cmd.buffer.registers[8].index = CPIA2_VP_RAM_DATA;
708                 cmd.buffer.registers[8].value = 0xFC;
709
710                 cmd.direction = TRANSFER_WRITE;
711                 cmd.reg_count = 9;
712
713                 cpia2_send_command(cam, &cmd);
714         }
715
716         /* Activate all settings and start the data stream */
717         /* Set user mode */
718         set_default_user_mode(cam);
719
720         /* Give VP time to wake up */
721         schedule_timeout_interruptible(msecs_to_jiffies(100));
722
723         set_all_properties(cam);
724
725         cpia2_do_command(cam, CPIA2_CMD_GET_USER_MODE, TRANSFER_READ, 0);
726         DBG("After SetAllProperties(cam), user mode is 0x%0X\n",
727             cam->params.vp_params.video_mode);
728
729         /***
730          * Set audio regulator off.  This and the code to set the compresison
731          * state are too complex to form a CPIA2_CMD_, and seem to be somewhat
732          * intertwined.  This stuff came straight from the windows driver.
733          ***/
734         /* Turn AutoExposure off in VP and enable the serial bridge to the sensor */
735         cpia2_do_command(cam, CPIA2_CMD_GET_VP_SYSTEM_CTRL, TRANSFER_READ, 0);
736         tmp_reg = cam->params.vp_params.system_ctrl;
737         cmd.buffer.registers[0].value = tmp_reg &
738                 (tmp_reg & (CPIA2_VP_SYSTEMCTRL_HK_CONTROL ^ 0xFF));
739
740         cpia2_do_command(cam, CPIA2_CMD_GET_DEVICE_CONFIG, TRANSFER_READ, 0);
741         cmd.buffer.registers[1].value = cam->params.vp_params.device_config |
742                                         CPIA2_VP_DEVICE_CONFIG_SERIAL_BRIDGE;
743         cmd.buffer.registers[0].index = CPIA2_VP_SYSTEMCTRL;
744         cmd.buffer.registers[1].index = CPIA2_VP_DEVICE_CONFIG;
745         cmd.req_mode = CAMERAACCESS_TYPE_RANDOM | CAMERAACCESS_VP;
746         cmd.reg_count = 2;
747         cmd.direction = TRANSFER_WRITE;
748         cmd.start = 0;
749         cpia2_send_command(cam, &cmd);
750
751         /* Set the correct I2C address in the CPiA-2 system register */
752         cpia2_do_command(cam,
753                          CPIA2_CMD_SET_SERIAL_ADDR,
754                          TRANSFER_WRITE,
755                          CPIA2_SYSTEM_VP_SERIAL_ADDR_SENSOR);
756
757         /* Now have sensor access - set bit to turn the audio regulator off */
758         cpia2_do_command(cam,
759                          CPIA2_CMD_SET_SENSOR_CR1,
760                          TRANSFER_WRITE, CPIA2_SENSOR_CR1_DOWN_AUDIO_REGULATOR);
761
762         /* Set the correct I2C address in the CPiA-2 system register */
763         if (cam->params.pnp_id.device_type == DEVICE_STV_672)
764                 cpia2_do_command(cam,
765                                  CPIA2_CMD_SET_SERIAL_ADDR,
766                                  TRANSFER_WRITE,
767                                  CPIA2_SYSTEM_VP_SERIAL_ADDR_VP); // 0x88
768         else
769                 cpia2_do_command(cam,
770                                  CPIA2_CMD_SET_SERIAL_ADDR,
771                                  TRANSFER_WRITE,
772                                  CPIA2_SYSTEM_VP_SERIAL_ADDR_676_VP); // 0x8a
773
774         /* increase signal drive strength */
775         if (cam->params.pnp_id.device_type == DEVICE_STV_676)
776                 cpia2_do_command(cam,
777                                  CPIA2_CMD_SET_VP_EXP_MODES,
778                                  TRANSFER_WRITE,
779                                  CPIA2_VP_EXPOSURE_MODES_COMPILE_EXP);
780
781         /* Start autoexposure */
782         cpia2_do_command(cam, CPIA2_CMD_GET_DEVICE_CONFIG, TRANSFER_READ, 0);
783         cmd.buffer.registers[0].value = cam->params.vp_params.device_config &
784                                   (CPIA2_VP_DEVICE_CONFIG_SERIAL_BRIDGE ^ 0xFF);
785
786         cpia2_do_command(cam, CPIA2_CMD_GET_VP_SYSTEM_CTRL, TRANSFER_READ, 0);
787         cmd.buffer.registers[1].value =
788             cam->params.vp_params.system_ctrl | CPIA2_VP_SYSTEMCTRL_HK_CONTROL;
789
790         cmd.buffer.registers[0].index = CPIA2_VP_DEVICE_CONFIG;
791         cmd.buffer.registers[1].index = CPIA2_VP_SYSTEMCTRL;
792         cmd.req_mode = CAMERAACCESS_TYPE_RANDOM | CAMERAACCESS_VP;
793         cmd.reg_count = 2;
794         cmd.direction = TRANSFER_WRITE;
795
796         cpia2_send_command(cam, &cmd);
797
798         /* Set compression state */
799         cpia2_do_command(cam, CPIA2_CMD_GET_VC_CONTROL, TRANSFER_READ, 0);
800         if (cam->params.compression.inhibit_htables) {
801                 tmp_reg = cam->params.vc_params.vc_control |
802                           CPIA2_VC_VC_CTRL_INHIBIT_H_TABLES;
803         } else  {
804                 tmp_reg = cam->params.vc_params.vc_control &
805                           ~CPIA2_VC_VC_CTRL_INHIBIT_H_TABLES;
806         }
807         cpia2_do_command(cam, CPIA2_CMD_SET_VC_CONTROL, TRANSFER_WRITE,tmp_reg);
808
809         /* Set target size (kb) on vc */
810         cpia2_do_command(cam, CPIA2_CMD_SET_TARGET_KB,
811                          TRANSFER_WRITE, cam->params.vc_params.target_kb);
812
813         /* Wiggle VC Reset */
814         /***
815          * First read and wait a bit.
816          ***/
817         for (i = 0; i < 50; i++) {
818                 cpia2_do_command(cam, CPIA2_CMD_GET_PW_CONTROL,
819                                  TRANSFER_READ, 0);
820         }
821
822         tmp_reg = cam->params.vc_params.pw_control;
823         tmp_reg &= ~CPIA2_VC_PW_CTRL_VC_RESET_N;
824
825         cpia2_do_command(cam, CPIA2_CMD_SET_PW_CONTROL, TRANSFER_WRITE,tmp_reg);
826
827         tmp_reg |= CPIA2_VC_PW_CTRL_VC_RESET_N;
828         cpia2_do_command(cam, CPIA2_CMD_SET_PW_CONTROL, TRANSFER_WRITE,tmp_reg);
829
830         cpia2_do_command(cam, CPIA2_CMD_SET_DEF_JPEG_OPT, TRANSFER_WRITE, 0);
831
832         cpia2_do_command(cam, CPIA2_CMD_GET_USER_MODE, TRANSFER_READ, 0);
833         DBG("After VC RESET, user mode is 0x%0X\n",
834             cam->params.vp_params.video_mode);
835
836         return retval;
837 }
838
839 /******************************************************************************
840  *
841  *  cpia2_set_high_power
842  *
843  *****************************************************************************/
844 static int cpia2_set_high_power(struct camera_data *cam)
845 {
846         int i;
847         for (i = 0; i <= 50; i++) {
848                 /* Read system status */
849                 cpia2_do_command(cam,CPIA2_CMD_GET_SYSTEM_CTRL,TRANSFER_READ,0);
850
851                 /* If there is an error, clear it */
852                 if(cam->params.camera_state.system_ctrl &
853                    CPIA2_SYSTEM_CONTROL_V2W_ERR)
854                         cpia2_do_command(cam, CPIA2_CMD_CLEAR_V2W_ERR,
855                                          TRANSFER_WRITE, 0);
856
857                 /* Try to set high power mode */
858                 cpia2_do_command(cam, CPIA2_CMD_SET_SYSTEM_CTRL,
859                                  TRANSFER_WRITE, 1);
860
861                 /* Try to read something in VP to check if everything is awake */
862                 cpia2_do_command(cam, CPIA2_CMD_GET_VP_SYSTEM_STATE,
863                                  TRANSFER_READ, 0);
864                 if (cam->params.vp_params.system_state &
865                     CPIA2_VP_SYSTEMSTATE_HK_ALIVE) {
866                         break;
867                 } else if (i == 50) {
868                         cam->params.camera_state.power_mode = LO_POWER_MODE;
869                         ERR("Camera did not wake up\n");
870                         return -EIO;
871                 }
872         }
873
874         DBG("System now in high power state\n");
875         cam->params.camera_state.power_mode = HI_POWER_MODE;
876         return 0;
877 }
878
879 /******************************************************************************
880  *
881  *  cpia2_set_low_power
882  *
883  *****************************************************************************/
884 int cpia2_set_low_power(struct camera_data *cam)
885 {
886         cam->params.camera_state.power_mode = LO_POWER_MODE;
887         cpia2_do_command(cam, CPIA2_CMD_SET_SYSTEM_CTRL, TRANSFER_WRITE, 0);
888         return 0;
889 }
890
891 /******************************************************************************
892  *
893  *  apply_vp_patch
894  *
895  *****************************************************************************/
896 static int apply_vp_patch(struct camera_data *cam)
897 {
898         int i, j;
899         struct cpia2_command cmd;
900
901         cmd.req_mode = CAMERAACCESS_TYPE_REPEAT | CAMERAACCESS_VP;
902         cmd.direction = TRANSFER_WRITE;
903
904         for (i = 0; i < PATCH_DATA_SIZE; i++) {
905                 for (j = 0; j < patch_data[i].count; j++) {
906                         cmd.buffer.block_data[j] = patch_data[i].data[j];
907                 }
908
909                 cmd.start = patch_data[i].reg;
910                 cmd.reg_count = patch_data[i].count;
911                 cpia2_send_command(cam, &cmd);
912         }
913
914         return 0;
915 }
916
917 /******************************************************************************
918  *
919  *  set_default_user_mode
920  *
921  *****************************************************************************/
922 static int set_default_user_mode(struct camera_data *cam)
923 {
924         unsigned char user_mode;
925         unsigned char frame_rate;
926         int width = cam->params.roi.width;
927         int height = cam->params.roi.height;
928
929         switch (cam->params.version.sensor_flags) {
930         case CPIA2_VP_SENSOR_FLAGS_404:
931         case CPIA2_VP_SENSOR_FLAGS_407:
932         case CPIA2_VP_SENSOR_FLAGS_409:
933         case CPIA2_VP_SENSOR_FLAGS_410:
934                 if ((width > STV_IMAGE_QCIF_COLS)
935                     || (height > STV_IMAGE_QCIF_ROWS)) {
936                         user_mode = CPIA2_VP_USER_MODE_CIF;
937                 } else {
938                         user_mode = CPIA2_VP_USER_MODE_QCIFDS;
939                 }
940                 frame_rate = CPIA2_VP_FRAMERATE_30;
941                 break;
942         case CPIA2_VP_SENSOR_FLAGS_500:
943                 if ((width > STV_IMAGE_CIF_COLS)
944                     || (height > STV_IMAGE_CIF_ROWS)) {
945                         user_mode = CPIA2_VP_USER_MODE_VGA;
946                 } else {
947                         user_mode = CPIA2_VP_USER_MODE_QVGADS;
948                 }
949                 if (cam->params.pnp_id.device_type == DEVICE_STV_672)
950                         frame_rate = CPIA2_VP_FRAMERATE_15;
951                 else
952                         frame_rate = CPIA2_VP_FRAMERATE_30;
953                 break;
954         default:
955                 LOG("%s: Invalid sensor flag value 0x%0X\n",__FUNCTION__,
956                     cam->params.version.sensor_flags);
957                 return -EINVAL;
958         }
959
960         DBG("Sensor flag = 0x%0x, user mode = 0x%0x, frame rate = 0x%X\n",
961             cam->params.version.sensor_flags, user_mode, frame_rate);
962         cpia2_do_command(cam, CPIA2_CMD_SET_USER_MODE, TRANSFER_WRITE,
963                          user_mode);
964         if(cam->params.vp_params.frame_rate > 0 &&
965            frame_rate > cam->params.vp_params.frame_rate)
966                 frame_rate = cam->params.vp_params.frame_rate;
967
968         cpia2_set_fps(cam, frame_rate);
969
970 //      if (cam->params.pnp_id.device_type == DEVICE_STV_676)
971 //              cpia2_do_command(cam,
972 //                               CPIA2_CMD_SET_VP_SYSTEM_CTRL,
973 //                               TRANSFER_WRITE,
974 //                               CPIA2_VP_SYSTEMCTRL_HK_CONTROL |
975 //                               CPIA2_VP_SYSTEMCTRL_POWER_CONTROL);
976
977         return 0;
978 }
979
980 /******************************************************************************
981  *
982  *  cpia2_match_video_size
983  *
984  *  return the best match, where 'best' is as always
985  *  the largest that is not bigger than what is requested.
986  *****************************************************************************/
987 int cpia2_match_video_size(int width, int height)
988 {
989         if (width >= STV_IMAGE_VGA_COLS && height >= STV_IMAGE_VGA_ROWS)
990                 return VIDEOSIZE_VGA;
991
992         if (width >= STV_IMAGE_CIF_COLS && height >= STV_IMAGE_CIF_ROWS)
993                 return VIDEOSIZE_CIF;
994
995         if (width >= STV_IMAGE_QVGA_COLS && height >= STV_IMAGE_QVGA_ROWS)
996                 return VIDEOSIZE_QVGA;
997
998         if (width >= 288 && height >= 216)
999                 return VIDEOSIZE_288_216;
1000
1001         if (width >= 256 && height >= 192)
1002                 return VIDEOSIZE_256_192;
1003
1004         if (width >= 224 && height >= 168)
1005                 return VIDEOSIZE_224_168;
1006
1007         if (width >= 192 && height >= 144)
1008                 return VIDEOSIZE_192_144;
1009
1010         if (width >= STV_IMAGE_QCIF_COLS && height >= STV_IMAGE_QCIF_ROWS)
1011                 return VIDEOSIZE_QCIF;
1012
1013         return -1;
1014 }
1015
1016 /******************************************************************************
1017  *
1018  *  SetVideoSize
1019  *
1020  *****************************************************************************/
1021 static int set_vw_size(struct camera_data *cam, int size)
1022 {
1023         int retval = 0;
1024
1025         cam->params.vp_params.video_size = size;
1026
1027         switch (size) {
1028         case VIDEOSIZE_VGA:
1029                 DBG("Setting size to VGA\n");
1030                 cam->params.roi.width = STV_IMAGE_VGA_COLS;
1031                 cam->params.roi.height = STV_IMAGE_VGA_ROWS;
1032                 cam->vw.width = STV_IMAGE_VGA_COLS;
1033                 cam->vw.height = STV_IMAGE_VGA_ROWS;
1034                 break;
1035         case VIDEOSIZE_CIF:
1036                 DBG("Setting size to CIF\n");
1037                 cam->params.roi.width = STV_IMAGE_CIF_COLS;
1038                 cam->params.roi.height = STV_IMAGE_CIF_ROWS;
1039                 cam->vw.width = STV_IMAGE_CIF_COLS;
1040                 cam->vw.height = STV_IMAGE_CIF_ROWS;
1041                 break;
1042         case VIDEOSIZE_QVGA:
1043                 DBG("Setting size to QVGA\n");
1044                 cam->params.roi.width = STV_IMAGE_QVGA_COLS;
1045                 cam->params.roi.height = STV_IMAGE_QVGA_ROWS;
1046                 cam->vw.width = STV_IMAGE_QVGA_COLS;
1047                 cam->vw.height = STV_IMAGE_QVGA_ROWS;
1048                 break;
1049         case VIDEOSIZE_288_216:
1050                 cam->params.roi.width = 288;
1051                 cam->params.roi.height = 216;
1052                 cam->vw.width = 288;
1053                 cam->vw.height = 216;
1054                 break;
1055         case VIDEOSIZE_256_192:
1056                 cam->vw.width = 256;
1057                 cam->vw.height = 192;
1058                 cam->params.roi.width = 256;
1059                 cam->params.roi.height = 192;
1060                 break;
1061         case VIDEOSIZE_224_168:
1062                 cam->vw.width = 224;
1063                 cam->vw.height = 168;
1064                 cam->params.roi.width = 224;
1065                 cam->params.roi.height = 168;
1066                 break;
1067         case VIDEOSIZE_192_144:
1068                 cam->vw.width = 192;
1069                 cam->vw.height = 144;
1070                 cam->params.roi.width = 192;
1071                 cam->params.roi.height = 144;
1072                 break;
1073         case VIDEOSIZE_QCIF:
1074                 DBG("Setting size to QCIF\n");
1075                 cam->params.roi.width = STV_IMAGE_QCIF_COLS;
1076                 cam->params.roi.height = STV_IMAGE_QCIF_ROWS;
1077                 cam->vw.width = STV_IMAGE_QCIF_COLS;
1078                 cam->vw.height = STV_IMAGE_QCIF_ROWS;
1079                 break;
1080         default:
1081                 retval = -EINVAL;
1082         }
1083         return retval;
1084 }
1085
1086 /******************************************************************************
1087  *
1088  *  configure_sensor
1089  *
1090  *****************************************************************************/
1091 static int configure_sensor(struct camera_data *cam,
1092                             int req_width, int req_height)
1093 {
1094         int retval;
1095
1096         switch (cam->params.version.sensor_flags) {
1097         case CPIA2_VP_SENSOR_FLAGS_404:
1098         case CPIA2_VP_SENSOR_FLAGS_407:
1099         case CPIA2_VP_SENSOR_FLAGS_409:
1100         case CPIA2_VP_SENSOR_FLAGS_410:
1101                 retval = config_sensor_410(cam, req_width, req_height);
1102                 break;
1103         case CPIA2_VP_SENSOR_FLAGS_500:
1104                 retval = config_sensor_500(cam, req_width, req_height);
1105                 break;
1106         default:
1107                 return -EINVAL;
1108         }
1109
1110         return retval;
1111 }
1112
1113 /******************************************************************************
1114  *
1115  *  config_sensor_410
1116  *
1117  *****************************************************************************/
1118 static int config_sensor_410(struct camera_data *cam,
1119                             int req_width, int req_height)
1120 {
1121         struct cpia2_command cmd;
1122         int i = 0;
1123         int image_size;
1124         int image_type;
1125         int width = req_width;
1126         int height = req_height;
1127
1128         /***
1129          *  Make sure size doesn't exceed CIF.
1130          ***/
1131         if (width > STV_IMAGE_CIF_COLS)
1132                 width = STV_IMAGE_CIF_COLS;
1133         if (height > STV_IMAGE_CIF_ROWS)
1134                 height = STV_IMAGE_CIF_ROWS;
1135
1136         image_size = cpia2_match_video_size(width, height);
1137
1138         DBG("Config 410: width = %d, height = %d\n", width, height);
1139         DBG("Image size returned is %d\n", image_size);
1140         if (image_size >= 0) {
1141                 set_vw_size(cam, image_size);
1142                 width = cam->params.roi.width;
1143                 height = cam->params.roi.height;
1144
1145                 DBG("After set_vw_size(), width = %d, height = %d\n",
1146                     width, height);
1147                 if (width <= 176 && height <= 144) {
1148                         DBG("image type = VIDEOSIZE_QCIF\n");
1149                         image_type = VIDEOSIZE_QCIF;
1150                 }
1151                 else if (width <= 320 && height <= 240) {
1152                         DBG("image type = VIDEOSIZE_QVGA\n");
1153                         image_type = VIDEOSIZE_QVGA;
1154                 }
1155                 else {
1156                         DBG("image type = VIDEOSIZE_CIF\n");
1157                         image_type = VIDEOSIZE_CIF;
1158                 }
1159         } else {
1160                 ERR("ConfigSensor410 failed\n");
1161                 return -EINVAL;
1162         }
1163
1164         cmd.req_mode = CAMERAACCESS_TYPE_RANDOM | CAMERAACCESS_VC;
1165         cmd.direction = TRANSFER_WRITE;
1166
1167         /* VC Format */
1168         cmd.buffer.registers[i].index = CPIA2_VC_VC_FORMAT;
1169         if (image_type == VIDEOSIZE_CIF) {
1170                 cmd.buffer.registers[i++].value =
1171                     (u8) (CPIA2_VC_VC_FORMAT_UFIRST |
1172                           CPIA2_VC_VC_FORMAT_SHORTLINE);
1173         } else {
1174                 cmd.buffer.registers[i++].value =
1175                     (u8) CPIA2_VC_VC_FORMAT_UFIRST;
1176         }
1177
1178         /* VC Clocks */
1179         cmd.buffer.registers[i].index = CPIA2_VC_VC_CLOCKS;
1180         if (image_type == VIDEOSIZE_QCIF) {
1181                 if (cam->params.pnp_id.device_type == DEVICE_STV_672) {
1182                         cmd.buffer.registers[i++].value=
1183                                 (u8)(CPIA2_VC_VC_672_CLOCKS_CIF_DIV_BY_3 |
1184                                      CPIA2_VC_VC_672_CLOCKS_SCALING |
1185                                      CPIA2_VC_VC_CLOCKS_LOGDIV2);
1186                         DBG("VC_Clocks (0xc4) should be B\n");
1187                 }
1188                 else {
1189                         cmd.buffer.registers[i++].value=
1190                                 (u8)(CPIA2_VC_VC_676_CLOCKS_CIF_DIV_BY_3 |
1191                                      CPIA2_VC_VC_CLOCKS_LOGDIV2);
1192                 }
1193         } else {
1194                 if (cam->params.pnp_id.device_type == DEVICE_STV_672) {
1195                         cmd.buffer.registers[i++].value =
1196                            (u8) (CPIA2_VC_VC_672_CLOCKS_CIF_DIV_BY_3 |
1197                                  CPIA2_VC_VC_CLOCKS_LOGDIV0);
1198                 }
1199                 else {
1200                         cmd.buffer.registers[i++].value =
1201                            (u8) (CPIA2_VC_VC_676_CLOCKS_CIF_DIV_BY_3 |
1202                                  CPIA2_VC_VC_676_CLOCKS_SCALING |
1203                                  CPIA2_VC_VC_CLOCKS_LOGDIV0);
1204                 }
1205         }
1206         DBG("VC_Clocks (0xc4) = 0x%0X\n", cmd.buffer.registers[i-1].value);
1207
1208         /* Input reqWidth from VC */
1209         cmd.buffer.registers[i].index = CPIA2_VC_VC_IHSIZE_LO;
1210         if (image_type == VIDEOSIZE_QCIF)
1211                 cmd.buffer.registers[i++].value =
1212                     (u8) (STV_IMAGE_QCIF_COLS / 4);
1213         else
1214                 cmd.buffer.registers[i++].value =
1215                     (u8) (STV_IMAGE_CIF_COLS / 4);
1216
1217         /* Timings */
1218         cmd.buffer.registers[i].index = CPIA2_VC_VC_XLIM_HI;
1219         if (image_type == VIDEOSIZE_QCIF)
1220                 cmd.buffer.registers[i++].value = (u8) 0;
1221         else
1222                 cmd.buffer.registers[i++].value = (u8) 1;
1223
1224         cmd.buffer.registers[i].index = CPIA2_VC_VC_XLIM_LO;
1225         if (image_type == VIDEOSIZE_QCIF)
1226                 cmd.buffer.registers[i++].value = (u8) 208;
1227         else
1228                 cmd.buffer.registers[i++].value = (u8) 160;
1229
1230         cmd.buffer.registers[i].index = CPIA2_VC_VC_YLIM_HI;
1231         if (image_type == VIDEOSIZE_QCIF)
1232                 cmd.buffer.registers[i++].value = (u8) 0;
1233         else
1234                 cmd.buffer.registers[i++].value = (u8) 1;
1235
1236         cmd.buffer.registers[i].index = CPIA2_VC_VC_YLIM_LO;
1237         if (image_type == VIDEOSIZE_QCIF)
1238                 cmd.buffer.registers[i++].value = (u8) 160;
1239         else
1240                 cmd.buffer.registers[i++].value = (u8) 64;
1241
1242         /* Output Image Size */
1243         cmd.buffer.registers[i].index = CPIA2_VC_VC_OHSIZE;
1244         cmd.buffer.registers[i++].value = cam->params.roi.width / 4;
1245
1246         cmd.buffer.registers[i].index = CPIA2_VC_VC_OVSIZE;
1247         cmd.buffer.registers[i++].value = cam->params.roi.height / 4;
1248
1249         /* Cropping */
1250         cmd.buffer.registers[i].index = CPIA2_VC_VC_HCROP;
1251         if (image_type == VIDEOSIZE_QCIF)
1252                 cmd.buffer.registers[i++].value =
1253                     (u8) (((STV_IMAGE_QCIF_COLS / 4) - (width / 4)) / 2);
1254         else
1255                 cmd.buffer.registers[i++].value =
1256                     (u8) (((STV_IMAGE_CIF_COLS / 4) - (width / 4)) / 2);
1257
1258         cmd.buffer.registers[i].index = CPIA2_VC_VC_VCROP;
1259         if (image_type == VIDEOSIZE_QCIF)
1260                 cmd.buffer.registers[i++].value =
1261                     (u8) (((STV_IMAGE_QCIF_ROWS / 4) - (height / 4)) / 2);
1262         else
1263                 cmd.buffer.registers[i++].value =
1264                     (u8) (((STV_IMAGE_CIF_ROWS / 4) - (height / 4)) / 2);
1265
1266         /* Scaling registers (defaults) */
1267         cmd.buffer.registers[i].index = CPIA2_VC_VC_HPHASE;
1268         cmd.buffer.registers[i++].value = (u8) 0;
1269
1270         cmd.buffer.registers[i].index = CPIA2_VC_VC_VPHASE;
1271         cmd.buffer.registers[i++].value = (u8) 0;
1272
1273         cmd.buffer.registers[i].index = CPIA2_VC_VC_HISPAN;
1274         cmd.buffer.registers[i++].value = (u8) 31;
1275
1276         cmd.buffer.registers[i].index = CPIA2_VC_VC_VISPAN;
1277         cmd.buffer.registers[i++].value = (u8) 31;
1278
1279         cmd.buffer.registers[i].index = CPIA2_VC_VC_HICROP;
1280         cmd.buffer.registers[i++].value = (u8) 0;
1281
1282         cmd.buffer.registers[i].index = CPIA2_VC_VC_VICROP;
1283         cmd.buffer.registers[i++].value = (u8) 0;
1284
1285         cmd.buffer.registers[i].index = CPIA2_VC_VC_HFRACT;
1286         cmd.buffer.registers[i++].value = (u8) 0x81;    /* = 8/1 = 8 (HIBYTE/LOBYTE) */
1287
1288         cmd.buffer.registers[i].index = CPIA2_VC_VC_VFRACT;
1289         cmd.buffer.registers[i++].value = (u8) 0x81;    /* = 8/1 = 8 (HIBYTE/LOBYTE) */
1290
1291         cmd.reg_count = i;
1292
1293         cpia2_send_command(cam, &cmd);
1294
1295         return i;
1296 }
1297
1298
1299 /******************************************************************************
1300  *
1301  *  config_sensor_500(cam)
1302  *
1303  *****************************************************************************/
1304 static int config_sensor_500(struct camera_data *cam,
1305                              int req_width, int req_height)
1306 {
1307         struct cpia2_command cmd;
1308         int i = 0;
1309         int image_size = VIDEOSIZE_CIF;
1310         int image_type = VIDEOSIZE_VGA;
1311         int width = req_width;
1312         int height = req_height;
1313         unsigned int device = cam->params.pnp_id.device_type;
1314
1315         image_size = cpia2_match_video_size(width, height);
1316
1317         if (width > STV_IMAGE_CIF_COLS || height > STV_IMAGE_CIF_ROWS)
1318                 image_type = VIDEOSIZE_VGA;
1319         else if (width > STV_IMAGE_QVGA_COLS || height > STV_IMAGE_QVGA_ROWS)
1320                 image_type = VIDEOSIZE_CIF;
1321         else if (width > STV_IMAGE_QCIF_COLS || height > STV_IMAGE_QCIF_ROWS)
1322                 image_type = VIDEOSIZE_QVGA;
1323         else
1324                 image_type = VIDEOSIZE_QCIF;
1325
1326         if (image_size >= 0) {
1327                 set_vw_size(cam, image_size);
1328                 width = cam->params.roi.width;
1329                 height = cam->params.roi.height;
1330         } else {
1331                 ERR("ConfigSensor500 failed\n");
1332                 return -EINVAL;
1333         }
1334
1335         DBG("image_size = %d, width = %d, height = %d, type = %d\n",
1336             image_size, width, height, image_type);
1337
1338         cmd.req_mode = CAMERAACCESS_TYPE_RANDOM | CAMERAACCESS_VC;
1339         cmd.direction = TRANSFER_WRITE;
1340         i = 0;
1341
1342         /* VC Format */
1343         cmd.buffer.registers[i].index = CPIA2_VC_VC_FORMAT;
1344         cmd.buffer.registers[i].value = (u8) CPIA2_VC_VC_FORMAT_UFIRST;
1345         if (image_type == VIDEOSIZE_QCIF)
1346                 cmd.buffer.registers[i].value |= (u8) CPIA2_VC_VC_FORMAT_DECIMATING;
1347         i++;
1348
1349         /* VC Clocks */
1350         cmd.buffer.registers[i].index = CPIA2_VC_VC_CLOCKS;
1351         if (device == DEVICE_STV_672) {
1352                 if (image_type == VIDEOSIZE_VGA)
1353                         cmd.buffer.registers[i].value =
1354                                 (u8)CPIA2_VC_VC_CLOCKS_LOGDIV1;
1355                 else
1356                         cmd.buffer.registers[i].value =
1357                                 (u8)(CPIA2_VC_VC_672_CLOCKS_SCALING |
1358                                      CPIA2_VC_VC_CLOCKS_LOGDIV3);
1359         } else {
1360                 if (image_type == VIDEOSIZE_VGA)
1361                         cmd.buffer.registers[i].value =
1362                                 (u8)CPIA2_VC_VC_CLOCKS_LOGDIV0;
1363                 else
1364                         cmd.buffer.registers[i].value =
1365                                 (u8)(CPIA2_VC_VC_676_CLOCKS_SCALING |
1366                                      CPIA2_VC_VC_CLOCKS_LOGDIV2);
1367         }
1368         i++;
1369
1370         DBG("VC_CLOCKS = 0x%X\n", cmd.buffer.registers[i-1].value);
1371
1372         /* Input width from VP */
1373         cmd.buffer.registers[i].index = CPIA2_VC_VC_IHSIZE_LO;
1374         if (image_type == VIDEOSIZE_VGA)
1375                 cmd.buffer.registers[i].value =
1376                     (u8) (STV_IMAGE_VGA_COLS / 4);
1377         else
1378                 cmd.buffer.registers[i].value =
1379                     (u8) (STV_IMAGE_QVGA_COLS / 4);
1380         i++;
1381         DBG("Input width = %d\n", cmd.buffer.registers[i-1].value);
1382
1383         /* Timings */
1384         cmd.buffer.registers[i].index = CPIA2_VC_VC_XLIM_HI;
1385         if (image_type == VIDEOSIZE_VGA)
1386                 cmd.buffer.registers[i++].value = (u8) 2;
1387         else
1388                 cmd.buffer.registers[i++].value = (u8) 1;
1389
1390         cmd.buffer.registers[i].index = CPIA2_VC_VC_XLIM_LO;
1391         if (image_type == VIDEOSIZE_VGA)
1392                 cmd.buffer.registers[i++].value = (u8) 250;
1393         else if (image_type == VIDEOSIZE_QVGA)
1394                 cmd.buffer.registers[i++].value = (u8) 125;
1395         else
1396                 cmd.buffer.registers[i++].value = (u8) 160;
1397
1398         cmd.buffer.registers[i].index = CPIA2_VC_VC_YLIM_HI;
1399         if (image_type == VIDEOSIZE_VGA)
1400                 cmd.buffer.registers[i++].value = (u8) 2;
1401         else
1402                 cmd.buffer.registers[i++].value = (u8) 1;
1403
1404         cmd.buffer.registers[i].index = CPIA2_VC_VC_YLIM_LO;
1405         if (image_type == VIDEOSIZE_VGA)
1406                 cmd.buffer.registers[i++].value = (u8) 12;
1407         else if (image_type == VIDEOSIZE_QVGA)
1408                 cmd.buffer.registers[i++].value = (u8) 64;
1409         else
1410                 cmd.buffer.registers[i++].value = (u8) 6;
1411
1412         /* Output Image Size */
1413         cmd.buffer.registers[i].index = CPIA2_VC_VC_OHSIZE;
1414         if (image_type == VIDEOSIZE_QCIF)
1415                 cmd.buffer.registers[i++].value = STV_IMAGE_CIF_COLS  / 4;
1416         else
1417                 cmd.buffer.registers[i++].value = width / 4;
1418
1419         cmd.buffer.registers[i].index = CPIA2_VC_VC_OVSIZE;
1420         if (image_type == VIDEOSIZE_QCIF)
1421                 cmd.buffer.registers[i++].value = STV_IMAGE_CIF_ROWS  / 4;
1422         else
1423                 cmd.buffer.registers[i++].value = height / 4;
1424
1425         /* Cropping */
1426         cmd.buffer.registers[i].index = CPIA2_VC_VC_HCROP;
1427         if (image_type == VIDEOSIZE_VGA)
1428                 cmd.buffer.registers[i++].value =
1429                     (u8) (((STV_IMAGE_VGA_COLS / 4) - (width / 4)) / 2);
1430         else if (image_type == VIDEOSIZE_QVGA)
1431                 cmd.buffer.registers[i++].value =
1432                     (u8) (((STV_IMAGE_QVGA_COLS / 4) - (width / 4)) / 2);
1433         else if (image_type == VIDEOSIZE_CIF)
1434                 cmd.buffer.registers[i++].value =
1435                     (u8) (((STV_IMAGE_CIF_COLS / 4) - (width / 4)) / 2);
1436         else /*if (image_type == VIDEOSIZE_QCIF)*/
1437                 cmd.buffer.registers[i++].value =
1438                         (u8) (((STV_IMAGE_QCIF_COLS / 4) - (width / 4)) / 2);
1439
1440         cmd.buffer.registers[i].index = CPIA2_VC_VC_VCROP;
1441         if (image_type == VIDEOSIZE_VGA)
1442                 cmd.buffer.registers[i++].value =
1443                     (u8) (((STV_IMAGE_VGA_ROWS / 4) - (height / 4)) / 2);
1444         else if (image_type == VIDEOSIZE_QVGA)
1445                 cmd.buffer.registers[i++].value =
1446                     (u8) (((STV_IMAGE_QVGA_ROWS / 4) - (height / 4)) / 2);
1447         else if (image_type == VIDEOSIZE_CIF)
1448                 cmd.buffer.registers[i++].value =
1449                     (u8) (((STV_IMAGE_CIF_ROWS / 4) - (height / 4)) / 2);
1450         else /*if (image_type == VIDEOSIZE_QCIF)*/
1451                 cmd.buffer.registers[i++].value =
1452                     (u8) (((STV_IMAGE_QCIF_ROWS / 4) - (height / 4)) / 2);
1453
1454         /* Scaling registers (defaults) */
1455         cmd.buffer.registers[i].index = CPIA2_VC_VC_HPHASE;
1456         if (image_type == VIDEOSIZE_CIF || image_type == VIDEOSIZE_QCIF)
1457                 cmd.buffer.registers[i++].value = (u8) 36;
1458         else
1459                 cmd.buffer.registers[i++].value = (u8) 0;
1460
1461         cmd.buffer.registers[i].index = CPIA2_VC_VC_VPHASE;
1462         if (image_type == VIDEOSIZE_CIF || image_type == VIDEOSIZE_QCIF)
1463                 cmd.buffer.registers[i++].value = (u8) 32;
1464         else
1465                 cmd.buffer.registers[i++].value = (u8) 0;
1466
1467         cmd.buffer.registers[i].index = CPIA2_VC_VC_HISPAN;
1468         if (image_type == VIDEOSIZE_CIF || image_type == VIDEOSIZE_QCIF)
1469                 cmd.buffer.registers[i++].value = (u8) 26;
1470         else
1471                 cmd.buffer.registers[i++].value = (u8) 31;
1472
1473         cmd.buffer.registers[i].index = CPIA2_VC_VC_VISPAN;
1474         if (image_type == VIDEOSIZE_CIF || image_type == VIDEOSIZE_QCIF)
1475                 cmd.buffer.registers[i++].value = (u8) 21;
1476         else
1477                 cmd.buffer.registers[i++].value = (u8) 31;
1478
1479         cmd.buffer.registers[i].index = CPIA2_VC_VC_HICROP;
1480         cmd.buffer.registers[i++].value = (u8) 0;
1481
1482         cmd.buffer.registers[i].index = CPIA2_VC_VC_VICROP;
1483         cmd.buffer.registers[i++].value = (u8) 0;
1484
1485         cmd.buffer.registers[i].index = CPIA2_VC_VC_HFRACT;
1486         if (image_type == VIDEOSIZE_CIF || image_type == VIDEOSIZE_QCIF)
1487                 cmd.buffer.registers[i++].value = (u8) 0x2B;    /* 2/11 */
1488         else
1489                 cmd.buffer.registers[i++].value = (u8) 0x81;    /* 8/1 */
1490
1491         cmd.buffer.registers[i].index = CPIA2_VC_VC_VFRACT;
1492         if (image_type == VIDEOSIZE_CIF || image_type == VIDEOSIZE_QCIF)
1493                 cmd.buffer.registers[i++].value = (u8) 0x13;    /* 1/3 */
1494         else
1495                 cmd.buffer.registers[i++].value = (u8) 0x81;    /* 8/1 */
1496
1497         cmd.reg_count = i;
1498
1499         cpia2_send_command(cam, &cmd);
1500
1501         return i;
1502 }
1503
1504
1505 /******************************************************************************
1506  *
1507  *  setallproperties
1508  *
1509  *  This sets all user changeable properties to the values in cam->params.
1510  *****************************************************************************/
1511 int set_all_properties(struct camera_data *cam)
1512 {
1513         /**
1514          * Don't set target_kb here, it will be set later.
1515          * framerate and user_mode were already set (set_default_user_mode).
1516          **/
1517
1518         cpia2_set_color_params(cam);
1519
1520         cpia2_usb_change_streaming_alternate(cam,
1521                                           cam->params.camera_state.stream_mode);
1522
1523         cpia2_do_command(cam, CPIA2_CMD_SET_USER_EFFECTS, TRANSFER_WRITE,
1524                          cam->params.vp_params.user_effects);
1525
1526         cpia2_set_flicker_mode(cam,
1527                                cam->params.flicker_control.flicker_mode_req);
1528
1529         cpia2_do_command(cam,
1530                          CPIA2_CMD_SET_VC_MP_GPIO_DIRECTION,
1531                          TRANSFER_WRITE, cam->params.vp_params.gpio_direction);
1532         cpia2_do_command(cam, CPIA2_CMD_SET_VC_MP_GPIO_DATA, TRANSFER_WRITE,
1533                          cam->params.vp_params.gpio_data);
1534
1535         wake_system(cam);
1536
1537         set_lowlight_boost(cam);
1538
1539         return 0;
1540 }
1541
1542 /******************************************************************************
1543  *
1544  *  cpia2_save_camera_state
1545  *
1546  *****************************************************************************/
1547 void cpia2_save_camera_state(struct camera_data *cam)
1548 {
1549         get_color_params(cam);
1550         cpia2_do_command(cam, CPIA2_CMD_GET_USER_EFFECTS, TRANSFER_READ, 0);
1551         cpia2_do_command(cam, CPIA2_CMD_GET_VC_MP_GPIO_DIRECTION, TRANSFER_READ,
1552                          0);
1553         cpia2_do_command(cam, CPIA2_CMD_GET_VC_MP_GPIO_DATA, TRANSFER_READ, 0);
1554         /* Don't get framerate or target_kb. Trust the values we already have */
1555 }
1556
1557 /******************************************************************************
1558  *
1559  *  get_color_params
1560  *
1561  *****************************************************************************/
1562 void get_color_params(struct camera_data *cam)
1563 {
1564         cpia2_do_command(cam, CPIA2_CMD_GET_VP_BRIGHTNESS, TRANSFER_READ, 0);
1565         cpia2_do_command(cam, CPIA2_CMD_GET_VP_SATURATION, TRANSFER_READ, 0);
1566         cpia2_do_command(cam, CPIA2_CMD_GET_CONTRAST, TRANSFER_READ, 0);
1567 }
1568
1569 /******************************************************************************
1570  *
1571  *  cpia2_set_color_params
1572  *
1573  *****************************************************************************/
1574 void cpia2_set_color_params(struct camera_data *cam)
1575 {
1576         DBG("Setting color params\n");
1577         cpia2_set_brightness(cam, cam->params.color_params.brightness);
1578         cpia2_set_contrast(cam, cam->params.color_params.contrast);
1579         cpia2_set_saturation(cam, cam->params.color_params.saturation);
1580 }
1581
1582 /******************************************************************************
1583  *
1584  *  cpia2_set_flicker_mode
1585  *
1586  *****************************************************************************/
1587 int cpia2_set_flicker_mode(struct camera_data *cam, int mode)
1588 {
1589         unsigned char cam_reg;
1590         int err = 0;
1591
1592         if(cam->params.pnp_id.device_type != DEVICE_STV_672)
1593                 return -EINVAL;
1594
1595         /* Set the appropriate bits in FLICKER_MODES, preserving the rest */
1596         if((err = cpia2_do_command(cam, CPIA2_CMD_GET_FLICKER_MODES,
1597                                    TRANSFER_READ, 0)))
1598                 return err;
1599         cam_reg = cam->params.flicker_control.cam_register;
1600
1601         switch(mode) {
1602         case NEVER_FLICKER:
1603                 cam_reg |= CPIA2_VP_FLICKER_MODES_NEVER_FLICKER;
1604                 cam_reg &= ~CPIA2_VP_FLICKER_MODES_50HZ;
1605                 break;
1606         case FLICKER_60:
1607                 cam_reg &= ~CPIA2_VP_FLICKER_MODES_NEVER_FLICKER;
1608                 cam_reg &= ~CPIA2_VP_FLICKER_MODES_50HZ;
1609                 break;
1610         case FLICKER_50:
1611                 cam_reg &= ~CPIA2_VP_FLICKER_MODES_NEVER_FLICKER;
1612                 cam_reg |= CPIA2_VP_FLICKER_MODES_50HZ;
1613                 break;
1614         default:
1615                 return -EINVAL;
1616         }
1617
1618         if((err = cpia2_do_command(cam, CPIA2_CMD_SET_FLICKER_MODES,
1619                                    TRANSFER_WRITE, cam_reg)))
1620                 return err;
1621
1622         /* Set the appropriate bits in EXP_MODES, preserving the rest */
1623         if((err = cpia2_do_command(cam, CPIA2_CMD_GET_VP_EXP_MODES,
1624                                    TRANSFER_READ, 0)))
1625                 return err;
1626         cam_reg = cam->params.vp_params.exposure_modes;
1627
1628         if (mode == NEVER_FLICKER) {
1629                 cam_reg |= CPIA2_VP_EXPOSURE_MODES_INHIBIT_FLICKER;
1630         } else {
1631                 cam_reg &= ~CPIA2_VP_EXPOSURE_MODES_INHIBIT_FLICKER;
1632         }
1633
1634         if((err = cpia2_do_command(cam, CPIA2_CMD_SET_VP_EXP_MODES,
1635                                    TRANSFER_WRITE, cam_reg)))
1636                 return err;
1637
1638         if((err = cpia2_do_command(cam, CPIA2_CMD_REHASH_VP4,
1639                                    TRANSFER_WRITE, 1)))
1640                 return err;
1641
1642         switch(mode) {
1643         case NEVER_FLICKER:
1644                 cam->params.flicker_control.flicker_mode_req = mode;
1645                 break;
1646         case FLICKER_60:
1647                 cam->params.flicker_control.flicker_mode_req = mode;
1648                 cam->params.flicker_control.mains_frequency = 60;
1649                 break;
1650         case FLICKER_50:
1651                 cam->params.flicker_control.flicker_mode_req = mode;
1652                 cam->params.flicker_control.mains_frequency = 50;
1653                 break;
1654         default:
1655                 err = -EINVAL;
1656         }
1657
1658         return err;
1659 }
1660
1661 /******************************************************************************
1662  *
1663  *  cpia2_set_property_flip
1664  *
1665  *****************************************************************************/
1666 void cpia2_set_property_flip(struct camera_data *cam, int prop_val)
1667 {
1668         unsigned char cam_reg;
1669
1670         cpia2_do_command(cam, CPIA2_CMD_GET_USER_EFFECTS, TRANSFER_READ, 0);
1671         cam_reg = cam->params.vp_params.user_effects;
1672
1673         if (prop_val)
1674         {
1675                 cam_reg |= CPIA2_VP_USER_EFFECTS_FLIP;
1676         }
1677         else
1678         {
1679                 cam_reg &= ~CPIA2_VP_USER_EFFECTS_FLIP;
1680         }
1681         cpia2_do_command(cam, CPIA2_CMD_SET_USER_EFFECTS, TRANSFER_WRITE,
1682                          cam_reg);
1683 }
1684
1685 /******************************************************************************
1686  *
1687  *  cpia2_set_property_mirror
1688  *
1689  *****************************************************************************/
1690 void cpia2_set_property_mirror(struct camera_data *cam, int prop_val)
1691 {
1692         unsigned char cam_reg;
1693
1694         cpia2_do_command(cam, CPIA2_CMD_GET_USER_EFFECTS, TRANSFER_READ, 0);
1695         cam_reg = cam->params.vp_params.user_effects;
1696
1697         if (prop_val)
1698         {
1699                 cam_reg |= CPIA2_VP_USER_EFFECTS_MIRROR;
1700         }
1701         else
1702         {
1703                 cam_reg &= ~CPIA2_VP_USER_EFFECTS_MIRROR;
1704         }
1705         cpia2_do_command(cam, CPIA2_CMD_SET_USER_EFFECTS, TRANSFER_WRITE,
1706                          cam_reg);
1707 }
1708
1709 /******************************************************************************
1710  *
1711  *  set_target_kb
1712  *
1713  *  The new Target KB is set in cam->params.vc_params.target_kb and
1714  *  activates on reset.
1715  *****************************************************************************/
1716
1717 int cpia2_set_target_kb(struct camera_data *cam, unsigned char value)
1718 {
1719         DBG("Requested target_kb = %d\n", value);
1720         if (value != cam->params.vc_params.target_kb) {
1721
1722                 cpia2_usb_stream_pause(cam);
1723
1724                 /* reset camera for new target_kb */
1725                 cam->params.vc_params.target_kb = value;
1726                 cpia2_reset_camera(cam);
1727
1728                 cpia2_usb_stream_resume(cam);
1729         }
1730
1731         return 0;
1732 }
1733
1734 /******************************************************************************
1735  *
1736  *  cpia2_set_gpio
1737  *
1738  *****************************************************************************/
1739 int cpia2_set_gpio(struct camera_data *cam, unsigned char setting)
1740 {
1741         int ret;
1742
1743         /* Set the microport direction (register 0x90, should be defined
1744          * already) to 1 (user output), and set the microport data (0x91) to
1745          * the value in the ioctl argument.
1746          */
1747
1748         ret = cpia2_do_command(cam,
1749                                CPIA2_CMD_SET_VC_MP_GPIO_DIRECTION,
1750                                CPIA2_VC_MP_DIR_OUTPUT,
1751                                255);
1752         if (ret < 0)
1753                 return ret;
1754         cam->params.vp_params.gpio_direction = 255;
1755
1756         ret = cpia2_do_command(cam,
1757                                CPIA2_CMD_SET_VC_MP_GPIO_DATA,
1758                                CPIA2_VC_MP_DIR_OUTPUT,
1759                                setting);
1760         if (ret < 0)
1761                 return ret;
1762         cam->params.vp_params.gpio_data = setting;
1763
1764         return 0;
1765 }
1766
1767 /******************************************************************************
1768  *
1769  *  cpia2_set_fps
1770  *
1771  *****************************************************************************/
1772 int cpia2_set_fps(struct camera_data *cam, int framerate)
1773 {
1774         int retval;
1775
1776         switch(framerate) {
1777                 case CPIA2_VP_FRAMERATE_30:
1778                 case CPIA2_VP_FRAMERATE_25:
1779                         if(cam->params.pnp_id.device_type == DEVICE_STV_672 &&
1780                            cam->params.version.sensor_flags ==
1781                                                     CPIA2_VP_SENSOR_FLAGS_500) {
1782                                 return -EINVAL;
1783                         }
1784                         /* Fall through */
1785                 case CPIA2_VP_FRAMERATE_15:
1786                 case CPIA2_VP_FRAMERATE_12_5:
1787                 case CPIA2_VP_FRAMERATE_7_5:
1788                 case CPIA2_VP_FRAMERATE_6_25:
1789                         break;
1790                 default:
1791                         return -EINVAL;
1792         }
1793
1794         if (cam->params.pnp_id.device_type == DEVICE_STV_672 &&
1795             framerate == CPIA2_VP_FRAMERATE_15)
1796                 framerate = 0; /* Work around bug in VP4 */
1797
1798         retval = cpia2_do_command(cam,
1799                                  CPIA2_CMD_FRAMERATE_REQ,
1800                                  TRANSFER_WRITE,
1801                                  framerate);
1802
1803         if(retval == 0)
1804                 cam->params.vp_params.frame_rate = framerate;
1805
1806         return retval;
1807 }
1808
1809 /******************************************************************************
1810  *
1811  *  cpia2_set_brightness
1812  *
1813  *****************************************************************************/
1814 void cpia2_set_brightness(struct camera_data *cam, unsigned char value)
1815 {
1816         /***
1817          * Don't let the register be set to zero - bug in VP4 - flash of full
1818          * brightness
1819          ***/
1820         if (cam->params.pnp_id.device_type == DEVICE_STV_672 && value == 0)
1821                 value++;
1822         DBG("Setting brightness to %d (0x%0x)\n", value, value);
1823         cpia2_do_command(cam,CPIA2_CMD_SET_VP_BRIGHTNESS, TRANSFER_WRITE,value);
1824 }
1825
1826 /******************************************************************************
1827  *
1828  *  cpia2_set_contrast
1829  *
1830  *****************************************************************************/
1831 void cpia2_set_contrast(struct camera_data *cam, unsigned char value)
1832 {
1833         DBG("Setting contrast to %d (0x%0x)\n", value, value);
1834         cam->params.color_params.contrast = value;
1835         cpia2_do_command(cam, CPIA2_CMD_SET_CONTRAST, TRANSFER_WRITE, value);
1836 }
1837
1838 /******************************************************************************
1839  *
1840  *  cpia2_set_saturation
1841  *
1842  *****************************************************************************/
1843 void cpia2_set_saturation(struct camera_data *cam, unsigned char value)
1844 {
1845         DBG("Setting saturation to %d (0x%0x)\n", value, value);
1846         cam->params.color_params.saturation = value;
1847         cpia2_do_command(cam,CPIA2_CMD_SET_VP_SATURATION, TRANSFER_WRITE,value);
1848 }
1849
1850 /******************************************************************************
1851  *
1852  *  wake_system
1853  *
1854  *****************************************************************************/
1855 void wake_system(struct camera_data *cam)
1856 {
1857         cpia2_do_command(cam, CPIA2_CMD_SET_WAKEUP, TRANSFER_WRITE, 0);
1858 }
1859
1860 /******************************************************************************
1861  *
1862  *  set_lowlight_boost
1863  *
1864  *  Valid for STV500 sensor only
1865  *****************************************************************************/
1866 void set_lowlight_boost(struct camera_data *cam)
1867 {
1868         struct cpia2_command cmd;
1869
1870         if (cam->params.pnp_id.device_type != DEVICE_STV_672 ||
1871             cam->params.version.sensor_flags != CPIA2_VP_SENSOR_FLAGS_500)
1872                 return;
1873
1874         cmd.direction = TRANSFER_WRITE;
1875         cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
1876         cmd.reg_count = 3;
1877         cmd.start = CPIA2_VP_RAM_ADDR_H;
1878
1879         cmd.buffer.block_data[0] = 0;   /* High byte of address to write to */
1880         cmd.buffer.block_data[1] = 0x59;        /* Low byte of address to write to */
1881         cmd.buffer.block_data[2] = 0;   /* High byte of data to write */
1882
1883         cpia2_send_command(cam, &cmd);
1884
1885         if (cam->params.vp_params.lowlight_boost) {
1886                 cmd.buffer.block_data[0] = 0x02;        /* Low byte data to write */
1887         } else {
1888                 cmd.buffer.block_data[0] = 0x06;
1889         }
1890         cmd.start = CPIA2_VP_RAM_DATA;
1891         cmd.reg_count = 1;
1892         cpia2_send_command(cam, &cmd);
1893
1894         /* Rehash the VP4 values */
1895         cpia2_do_command(cam, CPIA2_CMD_REHASH_VP4, TRANSFER_WRITE, 1);
1896 }
1897
1898 /******************************************************************************
1899  *
1900  *  cpia2_set_format
1901  *
1902  *  Assumes that new size is already set in param struct.
1903  *****************************************************************************/
1904 void cpia2_set_format(struct camera_data *cam)
1905 {
1906         cam->flush = true;
1907
1908         cpia2_usb_stream_pause(cam);
1909
1910         /* reset camera to new size */
1911         cpia2_set_low_power(cam);
1912         cpia2_reset_camera(cam);
1913         cam->flush = false;
1914
1915         cpia2_dbg_dump_registers(cam);
1916
1917         cpia2_usb_stream_resume(cam);
1918 }
1919
1920 /******************************************************************************
1921  *
1922  * cpia2_dbg_dump_registers
1923  *
1924  *****************************************************************************/
1925 void cpia2_dbg_dump_registers(struct camera_data *cam)
1926 {
1927 #ifdef _CPIA2_DEBUG_
1928         struct cpia2_command cmd;
1929
1930         if (!(debugs_on & DEBUG_DUMP_REGS))
1931                 return;
1932
1933         cmd.direction = TRANSFER_READ;
1934
1935         /* Start with bank 0 (SYSTEM) */
1936         cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_SYSTEM;
1937         cmd.reg_count = 3;
1938         cmd.start = 0;
1939         cpia2_send_command(cam, &cmd);
1940         printk(KERN_DEBUG "System Device Hi      = 0x%X\n",
1941                cmd.buffer.block_data[0]);
1942         printk(KERN_DEBUG "System Device Lo      = 0x%X\n",
1943                cmd.buffer.block_data[1]);
1944         printk(KERN_DEBUG "System_system control = 0x%X\n",
1945                cmd.buffer.block_data[2]);
1946
1947         /* Bank 1 (VC) */
1948         cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VC;
1949         cmd.reg_count = 4;
1950         cmd.start = 0x80;
1951         cpia2_send_command(cam, &cmd);
1952         printk(KERN_DEBUG "ASIC_ID       = 0x%X\n",
1953                cmd.buffer.block_data[0]);
1954         printk(KERN_DEBUG "ASIC_REV      = 0x%X\n",
1955                cmd.buffer.block_data[1]);
1956         printk(KERN_DEBUG "PW_CONTRL     = 0x%X\n",
1957                cmd.buffer.block_data[2]);
1958         printk(KERN_DEBUG "WAKEUP        = 0x%X\n",
1959                cmd.buffer.block_data[3]);
1960
1961         cmd.start = 0xA0;       /* ST_CTRL */
1962         cmd.reg_count = 1;
1963         cpia2_send_command(cam, &cmd);
1964         printk(KERN_DEBUG "Stream ctrl   = 0x%X\n",
1965                cmd.buffer.block_data[0]);
1966
1967         cmd.start = 0xA4;       /* Stream status */
1968         cpia2_send_command(cam, &cmd);
1969         printk(KERN_DEBUG "Stream status = 0x%X\n",
1970                cmd.buffer.block_data[0]);
1971
1972         cmd.start = 0xA8;       /* USB status */
1973         cmd.reg_count = 3;
1974         cpia2_send_command(cam, &cmd);
1975         printk(KERN_DEBUG "USB_CTRL      = 0x%X\n",
1976                cmd.buffer.block_data[0]);
1977         printk(KERN_DEBUG "USB_STRM      = 0x%X\n",
1978                cmd.buffer.block_data[1]);
1979         printk(KERN_DEBUG "USB_STATUS    = 0x%X\n",
1980                cmd.buffer.block_data[2]);
1981
1982         cmd.start = 0xAF;       /* USB settings */
1983         cmd.reg_count = 1;
1984         cpia2_send_command(cam, &cmd);
1985         printk(KERN_DEBUG "USB settings  = 0x%X\n",
1986                cmd.buffer.block_data[0]);
1987
1988         cmd.start = 0xC0;       /* VC stuff */
1989         cmd.reg_count = 26;
1990         cpia2_send_command(cam, &cmd);
1991         printk(KERN_DEBUG "VC Control    = 0x%0X\n",
1992                cmd.buffer.block_data[0]);
1993         printk(KERN_DEBUG "VC Format     = 0x%0X\n",
1994                cmd.buffer.block_data[3]);
1995         printk(KERN_DEBUG "VC Clocks     = 0x%0X\n",
1996                cmd.buffer.block_data[4]);
1997         printk(KERN_DEBUG "VC IHSize     = 0x%0X\n",
1998                cmd.buffer.block_data[5]);
1999         printk(KERN_DEBUG "VC Xlim Hi    = 0x%0X\n",
2000                cmd.buffer.block_data[6]);
2001         printk(KERN_DEBUG "VC XLim Lo    = 0x%0X\n",
2002                cmd.buffer.block_data[7]);
2003         printk(KERN_DEBUG "VC YLim Hi    = 0x%0X\n",
2004                cmd.buffer.block_data[8]);
2005         printk(KERN_DEBUG "VC YLim Lo    = 0x%0X\n",
2006                cmd.buffer.block_data[9]);
2007         printk(KERN_DEBUG "VC OHSize     = 0x%0X\n",
2008                cmd.buffer.block_data[10]);
2009         printk(KERN_DEBUG "VC OVSize     = 0x%0X\n",
2010                cmd.buffer.block_data[11]);
2011         printk(KERN_DEBUG "VC HCrop      = 0x%0X\n",
2012                cmd.buffer.block_data[12]);
2013         printk(KERN_DEBUG "VC VCrop      = 0x%0X\n",
2014                cmd.buffer.block_data[13]);
2015         printk(KERN_DEBUG "VC HPhase     = 0x%0X\n",
2016                cmd.buffer.block_data[14]);
2017         printk(KERN_DEBUG "VC VPhase     = 0x%0X\n",
2018                cmd.buffer.block_data[15]);
2019         printk(KERN_DEBUG "VC HIspan     = 0x%0X\n",
2020                cmd.buffer.block_data[16]);
2021         printk(KERN_DEBUG "VC VIspan     = 0x%0X\n",
2022                cmd.buffer.block_data[17]);
2023         printk(KERN_DEBUG "VC HiCrop     = 0x%0X\n",
2024                cmd.buffer.block_data[18]);
2025         printk(KERN_DEBUG "VC ViCrop     = 0x%0X\n",
2026                cmd.buffer.block_data[19]);
2027         printk(KERN_DEBUG "VC HiFract    = 0x%0X\n",
2028                cmd.buffer.block_data[20]);
2029         printk(KERN_DEBUG "VC ViFract    = 0x%0X\n",
2030                cmd.buffer.block_data[21]);
2031         printk(KERN_DEBUG "VC JPeg Opt   = 0x%0X\n",
2032                cmd.buffer.block_data[22]);
2033         printk(KERN_DEBUG "VC Creep Per  = 0x%0X\n",
2034                cmd.buffer.block_data[23]);
2035         printk(KERN_DEBUG "VC User Sq.   = 0x%0X\n",
2036                cmd.buffer.block_data[24]);
2037         printk(KERN_DEBUG "VC Target KB  = 0x%0X\n",
2038                cmd.buffer.block_data[25]);
2039
2040         /*** VP ***/
2041         cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
2042         cmd.reg_count = 14;
2043         cmd.start = 0;
2044         cpia2_send_command(cam, &cmd);
2045
2046         printk(KERN_DEBUG "VP Dev Hi     = 0x%0X\n",
2047                cmd.buffer.block_data[0]);
2048         printk(KERN_DEBUG "VP Dev Lo     = 0x%0X\n",
2049                cmd.buffer.block_data[1]);
2050         printk(KERN_DEBUG "VP Sys State  = 0x%0X\n",
2051                cmd.buffer.block_data[2]);
2052         printk(KERN_DEBUG "VP Sys Ctrl   = 0x%0X\n",
2053                cmd.buffer.block_data[3]);
2054         printk(KERN_DEBUG "VP Sensor flg = 0x%0X\n",
2055                cmd.buffer.block_data[5]);
2056         printk(KERN_DEBUG "VP Sensor Rev = 0x%0X\n",
2057                cmd.buffer.block_data[6]);
2058         printk(KERN_DEBUG "VP Dev Config = 0x%0X\n",
2059                cmd.buffer.block_data[7]);
2060         printk(KERN_DEBUG "VP GPIO_DIR   = 0x%0X\n",
2061                cmd.buffer.block_data[8]);
2062         printk(KERN_DEBUG "VP GPIO_DATA  = 0x%0X\n",
2063                cmd.buffer.block_data[9]);
2064         printk(KERN_DEBUG "VP Ram ADDR H = 0x%0X\n",
2065                cmd.buffer.block_data[10]);
2066         printk(KERN_DEBUG "VP Ram ADDR L = 0x%0X\n",
2067                cmd.buffer.block_data[11]);
2068         printk(KERN_DEBUG "VP RAM Data   = 0x%0X\n",
2069                cmd.buffer.block_data[12]);
2070         printk(KERN_DEBUG "Do Call       = 0x%0X\n",
2071                cmd.buffer.block_data[13]);
2072
2073         if (cam->params.pnp_id.device_type == DEVICE_STV_672) {
2074                 cmd.reg_count = 9;
2075                 cmd.start = 0x0E;
2076                 cpia2_send_command(cam, &cmd);
2077                 printk(KERN_DEBUG "VP Clock Ctrl = 0x%0X\n",
2078                        cmd.buffer.block_data[0]);
2079                 printk(KERN_DEBUG "VP Patch Rev  = 0x%0X\n",
2080                        cmd.buffer.block_data[1]);
2081                 printk(KERN_DEBUG "VP Vid Mode   = 0x%0X\n",
2082                        cmd.buffer.block_data[2]);
2083                 printk(KERN_DEBUG "VP Framerate  = 0x%0X\n",
2084                        cmd.buffer.block_data[3]);
2085                 printk(KERN_DEBUG "VP UserEffect = 0x%0X\n",
2086                        cmd.buffer.block_data[4]);
2087                 printk(KERN_DEBUG "VP White Bal  = 0x%0X\n",
2088                        cmd.buffer.block_data[5]);
2089                 printk(KERN_DEBUG "VP WB thresh  = 0x%0X\n",
2090                        cmd.buffer.block_data[6]);
2091                 printk(KERN_DEBUG "VP Exp Modes  = 0x%0X\n",
2092                        cmd.buffer.block_data[7]);
2093                 printk(KERN_DEBUG "VP Exp Target = 0x%0X\n",
2094                        cmd.buffer.block_data[8]);
2095
2096                 cmd.reg_count = 1;
2097                 cmd.start = 0x1B;
2098                 cpia2_send_command(cam, &cmd);
2099                 printk(KERN_DEBUG "VP FlickerMds = 0x%0X\n",
2100                        cmd.buffer.block_data[0]);
2101         } else {
2102                 cmd.reg_count = 8 ;
2103                 cmd.start = 0x0E;
2104                 cpia2_send_command(cam, &cmd);
2105                 printk(KERN_DEBUG "VP Clock Ctrl = 0x%0X\n",
2106                        cmd.buffer.block_data[0]);
2107                 printk(KERN_DEBUG "VP Patch Rev  = 0x%0X\n",
2108                        cmd.buffer.block_data[1]);
2109                 printk(KERN_DEBUG "VP Vid Mode   = 0x%0X\n",
2110                        cmd.buffer.block_data[5]);
2111                 printk(KERN_DEBUG "VP Framerate  = 0x%0X\n",
2112                        cmd.buffer.block_data[6]);
2113                 printk(KERN_DEBUG "VP UserEffect = 0x%0X\n",
2114                        cmd.buffer.block_data[7]);
2115
2116                 cmd.reg_count = 1;
2117                 cmd.start = CPIA2_VP5_EXPOSURE_TARGET;
2118                 cpia2_send_command(cam, &cmd);
2119                 printk(KERN_DEBUG "VP5 Exp Target= 0x%0X\n",
2120                        cmd.buffer.block_data[0]);
2121
2122                 cmd.reg_count = 4;
2123                 cmd.start = 0x3A;
2124                 cpia2_send_command(cam, &cmd);
2125                 printk(KERN_DEBUG "VP5 MY Black  = 0x%0X\n",
2126                        cmd.buffer.block_data[0]);
2127                 printk(KERN_DEBUG "VP5 MCY Range = 0x%0X\n",
2128                        cmd.buffer.block_data[1]);
2129                 printk(KERN_DEBUG "VP5 MYCEILING = 0x%0X\n",
2130                        cmd.buffer.block_data[2]);
2131                 printk(KERN_DEBUG "VP5 MCUV Sat  = 0x%0X\n",
2132                        cmd.buffer.block_data[3]);
2133         }
2134 #endif
2135 }
2136
2137 /******************************************************************************
2138  *
2139  *  reset_camera_struct
2140  *
2141  *  Sets all values to the defaults
2142  *****************************************************************************/
2143 void reset_camera_struct(struct camera_data *cam)
2144 {
2145         /***
2146          * The following parameter values are the defaults from the register map.
2147          ***/
2148         cam->params.color_params.brightness = DEFAULT_BRIGHTNESS;
2149         cam->params.color_params.contrast = DEFAULT_CONTRAST;
2150         cam->params.color_params.saturation = DEFAULT_SATURATION;
2151         cam->params.vp_params.lowlight_boost = 0;
2152
2153         /* FlickerModes */
2154         cam->params.flicker_control.flicker_mode_req = NEVER_FLICKER;
2155         cam->params.flicker_control.mains_frequency = 60;
2156
2157         /* jpeg params */
2158         cam->params.compression.jpeg_options = CPIA2_VC_VC_JPEG_OPT_DEFAULT;
2159         cam->params.compression.creep_period = 2;
2160         cam->params.compression.user_squeeze = 20;
2161         cam->params.compression.inhibit_htables = false;
2162
2163         /* gpio params */
2164         cam->params.vp_params.gpio_direction = 0;       /* write, the default safe mode */
2165         cam->params.vp_params.gpio_data = 0;
2166
2167         /* Target kb params */
2168         cam->params.vc_params.target_kb = DEFAULT_TARGET_KB;
2169
2170         /***
2171          * Set Sensor FPS as fast as possible.
2172          ***/
2173         if(cam->params.pnp_id.device_type == DEVICE_STV_672) {
2174                 if(cam->params.version.sensor_flags == CPIA2_VP_SENSOR_FLAGS_500)
2175                         cam->params.vp_params.frame_rate = CPIA2_VP_FRAMERATE_15;
2176                 else
2177                         cam->params.vp_params.frame_rate = CPIA2_VP_FRAMERATE_30;
2178         } else {
2179                 cam->params.vp_params.frame_rate = CPIA2_VP_FRAMERATE_30;
2180         }
2181
2182         /***
2183          * Set default video mode as large as possible :
2184          * for vga sensor set to vga, for cif sensor set to CIF.
2185          ***/
2186         if (cam->params.version.sensor_flags == CPIA2_VP_SENSOR_FLAGS_500) {
2187                 cam->sensor_type = CPIA2_SENSOR_500;
2188                 cam->video_size = VIDEOSIZE_VGA;
2189                 cam->params.roi.width = STV_IMAGE_VGA_COLS;
2190                 cam->params.roi.height = STV_IMAGE_VGA_ROWS;
2191         } else {
2192                 cam->sensor_type = CPIA2_SENSOR_410;
2193                 cam->video_size = VIDEOSIZE_CIF;
2194                 cam->params.roi.width = STV_IMAGE_CIF_COLS;
2195                 cam->params.roi.height = STV_IMAGE_CIF_ROWS;
2196         }
2197
2198         /***
2199          * Fill in the v4l structures.  video_cap is filled in inside the VIDIOCCAP
2200          * Ioctl.  Here, just do the window and picture stucts.
2201          ***/
2202         cam->vp.palette = (u16) VIDEO_PALETTE_RGB24;    /* Is this right? */
2203         cam->vp.brightness = (u16) cam->params.color_params.brightness * 256;
2204         cam->vp.colour = (u16) cam->params.color_params.saturation * 256;
2205         cam->vp.contrast = (u16) cam->params.color_params.contrast * 256;
2206
2207         cam->vw.x = 0;
2208         cam->vw.y = 0;
2209         cam->vw.width = cam->params.roi.width;
2210         cam->vw.height = cam->params.roi.height;
2211         cam->vw.flags = 0;
2212         cam->vw.clipcount = 0;
2213
2214         return;
2215 }
2216
2217 /******************************************************************************
2218  *
2219  *  cpia2_init_camera_struct
2220  *
2221  *  Initializes camera struct, does not call reset to fill in defaults.
2222  *****************************************************************************/
2223 struct camera_data *cpia2_init_camera_struct(void)
2224 {
2225         struct camera_data *cam;
2226
2227         cam = kzalloc(sizeof(*cam), GFP_KERNEL);
2228
2229         if (!cam) {
2230                 ERR("couldn't kmalloc cpia2 struct\n");
2231                 return NULL;
2232         }
2233
2234
2235         cam->present = 1;
2236         mutex_init(&cam->busy_lock);
2237         init_waitqueue_head(&cam->wq_stream);
2238
2239         return cam;
2240 }
2241
2242 /******************************************************************************
2243  *
2244  *  cpia2_init_camera
2245  *
2246  *  Initializes camera.
2247  *****************************************************************************/
2248 int cpia2_init_camera(struct camera_data *cam)
2249 {
2250         DBG("Start\n");
2251
2252         cam->mmapped = false;
2253
2254         /* Get sensor and asic types before reset. */
2255         cpia2_set_high_power(cam);
2256         cpia2_get_version_info(cam);
2257         if (cam->params.version.asic_id != CPIA2_ASIC_672) {
2258                 ERR("Device IO error (asicID has incorrect value of 0x%X\n",
2259                     cam->params.version.asic_id);
2260                 return -ENODEV;
2261         }
2262
2263         /* Set GPIO direction and data to a safe state. */
2264         cpia2_do_command(cam, CPIA2_CMD_SET_VC_MP_GPIO_DIRECTION,
2265                          TRANSFER_WRITE, 0);
2266         cpia2_do_command(cam, CPIA2_CMD_SET_VC_MP_GPIO_DATA,
2267                          TRANSFER_WRITE, 0);
2268
2269         /* resetting struct requires version info for sensor and asic types */
2270         reset_camera_struct(cam);
2271
2272         cpia2_set_low_power(cam);
2273
2274         DBG("End\n");
2275
2276         return 0;
2277 }
2278
2279 /******************************************************************************
2280  *
2281  *  cpia2_allocate_buffers
2282  *
2283  *****************************************************************************/
2284 int cpia2_allocate_buffers(struct camera_data *cam)
2285 {
2286         int i;
2287
2288         if(!cam->buffers) {
2289                 u32 size = cam->num_frames*sizeof(struct framebuf);
2290                 cam->buffers = kmalloc(size, GFP_KERNEL);
2291                 if(!cam->buffers) {
2292                         ERR("couldn't kmalloc frame buffer structures\n");
2293                         return -ENOMEM;
2294                 }
2295         }
2296
2297         if(!cam->frame_buffer) {
2298                 cam->frame_buffer = rvmalloc(cam->frame_size*cam->num_frames);
2299                 if (!cam->frame_buffer) {
2300                         ERR("couldn't vmalloc frame buffer data area\n");
2301                         kfree(cam->buffers);
2302                         cam->buffers = NULL;
2303                         return -ENOMEM;
2304                 }
2305         }
2306
2307         for(i=0; i<cam->num_frames-1; ++i) {
2308                 cam->buffers[i].next = &cam->buffers[i+1];
2309                 cam->buffers[i].data = cam->frame_buffer +i*cam->frame_size;
2310                 cam->buffers[i].status = FRAME_EMPTY;
2311                 cam->buffers[i].length = 0;
2312                 cam->buffers[i].max_length = 0;
2313                 cam->buffers[i].num = i;
2314         }
2315         cam->buffers[i].next = cam->buffers;
2316         cam->buffers[i].data = cam->frame_buffer +i*cam->frame_size;
2317         cam->buffers[i].status = FRAME_EMPTY;
2318         cam->buffers[i].length = 0;
2319         cam->buffers[i].max_length = 0;
2320         cam->buffers[i].num = i;
2321         cam->curbuff = cam->buffers;
2322         cam->workbuff = cam->curbuff->next;
2323         DBG("buffers=%p, curbuff=%p, workbuff=%p\n", cam->buffers, cam->curbuff,
2324             cam->workbuff);
2325         return 0;
2326 }
2327
2328 /******************************************************************************
2329  *
2330  *  cpia2_free_buffers
2331  *
2332  *****************************************************************************/
2333 void cpia2_free_buffers(struct camera_data *cam)
2334 {
2335         if(cam->buffers) {
2336                 kfree(cam->buffers);
2337                 cam->buffers = NULL;
2338         }
2339         if(cam->frame_buffer) {
2340                 rvfree(cam->frame_buffer, cam->frame_size*cam->num_frames);
2341                 cam->frame_buffer = NULL;
2342         }
2343 }
2344
2345 /******************************************************************************
2346  *
2347  *  cpia2_read
2348  *
2349  *****************************************************************************/
2350 long cpia2_read(struct camera_data *cam,
2351                 char __user *buf, unsigned long count, int noblock)
2352 {
2353         struct framebuf *frame;
2354         if (!count) {
2355                 return 0;
2356         }
2357
2358         if (!buf) {
2359                 ERR("%s: buffer NULL\n",__FUNCTION__);
2360                 return -EINVAL;
2361         }
2362
2363         if (!cam) {
2364                 ERR("%s: Internal error, camera_data NULL!\n",__FUNCTION__);
2365                 return -EINVAL;
2366         }
2367
2368         /* make this _really_ smp and multithread-safe */
2369         if (mutex_lock_interruptible(&cam->busy_lock))
2370                 return -ERESTARTSYS;
2371
2372         if (!cam->present) {
2373                 LOG("%s: camera removed\n",__FUNCTION__);
2374                 mutex_unlock(&cam->busy_lock);
2375                 return 0;       /* EOF */
2376         }
2377
2378         if(!cam->streaming) {
2379                 /* Start streaming */
2380                 cpia2_usb_stream_start(cam,
2381                                        cam->params.camera_state.stream_mode);
2382         }
2383
2384         /* Copy cam->curbuff in case it changes while we're processing */
2385         frame = cam->curbuff;
2386         if (noblock && frame->status != FRAME_READY) {
2387                 mutex_unlock(&cam->busy_lock);
2388                 return -EAGAIN;
2389         }
2390
2391         if(frame->status != FRAME_READY) {
2392                 mutex_unlock(&cam->busy_lock);
2393                 wait_event_interruptible(cam->wq_stream,
2394                                !cam->present ||
2395                                (frame = cam->curbuff)->status == FRAME_READY);
2396                 if (signal_pending(current))
2397                         return -ERESTARTSYS;
2398                 /* make this _really_ smp and multithread-safe */
2399                 if (mutex_lock_interruptible(&cam->busy_lock)) {
2400                         return -ERESTARTSYS;
2401                 }
2402                 if(!cam->present) {
2403                         mutex_unlock(&cam->busy_lock);
2404                         return 0;
2405                 }
2406         }
2407
2408         /* copy data to user space */
2409         if (frame->length > count) {
2410                 mutex_unlock(&cam->busy_lock);
2411                 return -EFAULT;
2412         }
2413         if (copy_to_user(buf, frame->data, frame->length)) {
2414                 mutex_unlock(&cam->busy_lock);
2415                 return -EFAULT;
2416         }
2417
2418         count = frame->length;
2419
2420         frame->status = FRAME_EMPTY;
2421
2422         mutex_unlock(&cam->busy_lock);
2423         return count;
2424 }
2425
2426 /******************************************************************************
2427  *
2428  *  cpia2_poll
2429  *
2430  *****************************************************************************/
2431 unsigned int cpia2_poll(struct camera_data *cam, struct file *filp,
2432                         poll_table *wait)
2433 {
2434         unsigned int status=0;
2435
2436         if(!cam) {
2437                 ERR("%s: Internal error, camera_data not found!\n",__FUNCTION__);
2438                 return POLLERR;
2439         }
2440
2441         mutex_lock(&cam->busy_lock);
2442
2443         if(!cam->present) {
2444                 mutex_unlock(&cam->busy_lock);
2445                 return POLLHUP;
2446         }
2447
2448         if(!cam->streaming) {
2449                 /* Start streaming */
2450                 cpia2_usb_stream_start(cam,
2451                                        cam->params.camera_state.stream_mode);
2452         }
2453
2454         mutex_unlock(&cam->busy_lock);
2455         poll_wait(filp, &cam->wq_stream, wait);
2456         mutex_lock(&cam->busy_lock);
2457
2458         if(!cam->present)
2459                 status = POLLHUP;
2460         else if(cam->curbuff->status == FRAME_READY)
2461                 status = POLLIN | POLLRDNORM;
2462
2463         mutex_unlock(&cam->busy_lock);
2464         return status;
2465 }
2466
2467 /******************************************************************************
2468  *
2469  *  cpia2_remap_buffer
2470  *
2471  *****************************************************************************/
2472 int cpia2_remap_buffer(struct camera_data *cam, struct vm_area_struct *vma)
2473 {
2474         const char *adr = (const char *)vma->vm_start;
2475         unsigned long size = vma->vm_end-vma->vm_start;
2476         unsigned long start_offset = vma->vm_pgoff << PAGE_SHIFT;
2477         unsigned long start = (unsigned long) adr;
2478         unsigned long page, pos;
2479
2480         if (!cam)
2481                 return -ENODEV;
2482
2483         DBG("mmap offset:%ld size:%ld\n", start_offset, size);
2484
2485         /* make this _really_ smp-safe */
2486         if (mutex_lock_interruptible(&cam->busy_lock))
2487                 return -ERESTARTSYS;
2488
2489         if (!cam->present) {
2490                 mutex_unlock(&cam->busy_lock);
2491                 return -ENODEV;
2492         }
2493
2494         if (size > cam->frame_size*cam->num_frames  ||
2495             (start_offset % cam->frame_size) != 0 ||
2496             (start_offset+size > cam->frame_size*cam->num_frames)) {
2497                 mutex_unlock(&cam->busy_lock);
2498                 return -EINVAL;
2499         }
2500
2501         pos = ((unsigned long) (cam->frame_buffer)) + start_offset;
2502         while (size > 0) {
2503                 page = kvirt_to_pa(pos);
2504                 if (remap_pfn_range(vma, start, page >> PAGE_SHIFT, PAGE_SIZE, PAGE_SHARED)) {
2505                         mutex_unlock(&cam->busy_lock);
2506                         return -EAGAIN;
2507                 }
2508                 start += PAGE_SIZE;
2509                 pos += PAGE_SIZE;
2510                 if (size > PAGE_SIZE)
2511                         size -= PAGE_SIZE;
2512                 else
2513                         size = 0;
2514         }
2515
2516         cam->mmapped = true;
2517         mutex_unlock(&cam->busy_lock);
2518         return 0;
2519 }
2520