Merge branch 'upstream-linus' of git://oss.oracle.com/home/sourcebo/git/ocfs2
[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         current->state = TASK_INTERRUPTIBLE;
667         schedule_timeout(100 * HZ / 1000);      /* wait for 100 msecs */
668
669         if (cam->params.pnp_id.device_type == DEVICE_STV_672)
670                 retval = apply_vp_patch(cam);
671
672         /* wait for vp to go to sleep */
673         current->state = TASK_INTERRUPTIBLE;
674         schedule_timeout(100 * HZ / 1000);      /* wait for 100 msecs */
675
676         /***
677          * If this is a 676, apply VP5 fixes before we start streaming
678          ***/
679         if (cam->params.pnp_id.device_type == DEVICE_STV_676) {
680                 cmd.req_mode = CAMERAACCESS_TYPE_RANDOM | CAMERAACCESS_VP;
681
682                 /* The following writes improve the picture */
683                 cmd.buffer.registers[0].index = CPIA2_VP5_MYBLACK_LEVEL;
684                 cmd.buffer.registers[0].value = 0; /* reduce from the default
685                                                     * rec 601 pedestal of 16 */
686                 cmd.buffer.registers[1].index = CPIA2_VP5_MCYRANGE;
687                 cmd.buffer.registers[1].value = 0x92; /* increase from 100% to
688                                                        * (256/256 - 31) to fill
689                                                        * available range */
690                 cmd.buffer.registers[2].index = CPIA2_VP5_MYCEILING;
691                 cmd.buffer.registers[2].value = 0xFF; /* Increase from the
692                                                        * default rec 601 ceiling
693                                                        * of 240 */
694                 cmd.buffer.registers[3].index = CPIA2_VP5_MCUVSATURATION;
695                 cmd.buffer.registers[3].value = 0xFF; /* Increase from the rec
696                                                        * 601 100% level (128)
697                                                        * to 145-192 */
698                 cmd.buffer.registers[4].index = CPIA2_VP5_ANTIFLKRSETUP;
699                 cmd.buffer.registers[4].value = 0x80;  /* Inhibit the
700                                                         * anti-flicker */
701
702                 /* The following 4 writes are a fix to allow QVGA to work at 30 fps */
703                 cmd.buffer.registers[5].index = CPIA2_VP_RAM_ADDR_H;
704                 cmd.buffer.registers[5].value = 0x01;
705                 cmd.buffer.registers[6].index = CPIA2_VP_RAM_ADDR_L;
706                 cmd.buffer.registers[6].value = 0xE3;
707                 cmd.buffer.registers[7].index = CPIA2_VP_RAM_DATA;
708                 cmd.buffer.registers[7].value = 0x02;
709                 cmd.buffer.registers[8].index = CPIA2_VP_RAM_DATA;
710                 cmd.buffer.registers[8].value = 0xFC;
711
712                 cmd.direction = TRANSFER_WRITE;
713                 cmd.reg_count = 9;
714
715                 cpia2_send_command(cam, &cmd);
716         }
717
718         /* Activate all settings and start the data stream */
719         /* Set user mode */
720         set_default_user_mode(cam);
721
722         /* Give VP time to wake up */
723         current->state = TASK_INTERRUPTIBLE;
724         schedule_timeout(100 * HZ / 1000);      /* wait for 100 msecs */
725
726         set_all_properties(cam);
727
728         cpia2_do_command(cam, CPIA2_CMD_GET_USER_MODE, TRANSFER_READ, 0);
729         DBG("After SetAllProperties(cam), user mode is 0x%0X\n",
730             cam->params.vp_params.video_mode);
731
732         /***
733          * Set audio regulator off.  This and the code to set the compresison
734          * state are too complex to form a CPIA2_CMD_, and seem to be somewhat
735          * intertwined.  This stuff came straight from the windows driver.
736          ***/
737         /* Turn AutoExposure off in VP and enable the serial bridge to the sensor */
738         cpia2_do_command(cam, CPIA2_CMD_GET_VP_SYSTEM_CTRL, TRANSFER_READ, 0);
739         tmp_reg = cam->params.vp_params.system_ctrl;
740         cmd.buffer.registers[0].value = tmp_reg &
741                 (tmp_reg & (CPIA2_VP_SYSTEMCTRL_HK_CONTROL ^ 0xFF));
742
743         cpia2_do_command(cam, CPIA2_CMD_GET_DEVICE_CONFIG, TRANSFER_READ, 0);
744         cmd.buffer.registers[1].value = cam->params.vp_params.device_config |
745                                         CPIA2_VP_DEVICE_CONFIG_SERIAL_BRIDGE;
746         cmd.buffer.registers[0].index = CPIA2_VP_SYSTEMCTRL;
747         cmd.buffer.registers[1].index = CPIA2_VP_DEVICE_CONFIG;
748         cmd.req_mode = CAMERAACCESS_TYPE_RANDOM | CAMERAACCESS_VP;
749         cmd.reg_count = 2;
750         cmd.direction = TRANSFER_WRITE;
751         cmd.start = 0;
752         cpia2_send_command(cam, &cmd);
753
754         /* Set the correct I2C address in the CPiA-2 system register */
755         cpia2_do_command(cam,
756                          CPIA2_CMD_SET_SERIAL_ADDR,
757                          TRANSFER_WRITE,
758                          CPIA2_SYSTEM_VP_SERIAL_ADDR_SENSOR);
759
760         /* Now have sensor access - set bit to turn the audio regulator off */
761         cpia2_do_command(cam,
762                          CPIA2_CMD_SET_SENSOR_CR1,
763                          TRANSFER_WRITE, CPIA2_SENSOR_CR1_DOWN_AUDIO_REGULATOR);
764
765         /* Set the correct I2C address in the CPiA-2 system register */
766         if (cam->params.pnp_id.device_type == DEVICE_STV_672)
767                 cpia2_do_command(cam,
768                                  CPIA2_CMD_SET_SERIAL_ADDR,
769                                  TRANSFER_WRITE,
770                                  CPIA2_SYSTEM_VP_SERIAL_ADDR_VP); // 0x88
771         else
772                 cpia2_do_command(cam,
773                                  CPIA2_CMD_SET_SERIAL_ADDR,
774                                  TRANSFER_WRITE,
775                                  CPIA2_SYSTEM_VP_SERIAL_ADDR_676_VP); // 0x8a
776
777         /* increase signal drive strength */
778         if (cam->params.pnp_id.device_type == DEVICE_STV_676)
779                 cpia2_do_command(cam,
780                                  CPIA2_CMD_SET_VP_EXP_MODES,
781                                  TRANSFER_WRITE,
782                                  CPIA2_VP_EXPOSURE_MODES_COMPILE_EXP);
783
784         /* Start autoexposure */
785         cpia2_do_command(cam, CPIA2_CMD_GET_DEVICE_CONFIG, TRANSFER_READ, 0);
786         cmd.buffer.registers[0].value = cam->params.vp_params.device_config &
787                                   (CPIA2_VP_DEVICE_CONFIG_SERIAL_BRIDGE ^ 0xFF);
788
789         cpia2_do_command(cam, CPIA2_CMD_GET_VP_SYSTEM_CTRL, TRANSFER_READ, 0);
790         cmd.buffer.registers[1].value =
791             cam->params.vp_params.system_ctrl | CPIA2_VP_SYSTEMCTRL_HK_CONTROL;
792
793         cmd.buffer.registers[0].index = CPIA2_VP_DEVICE_CONFIG;
794         cmd.buffer.registers[1].index = CPIA2_VP_SYSTEMCTRL;
795         cmd.req_mode = CAMERAACCESS_TYPE_RANDOM | CAMERAACCESS_VP;
796         cmd.reg_count = 2;
797         cmd.direction = TRANSFER_WRITE;
798
799         cpia2_send_command(cam, &cmd);
800
801         /* Set compression state */
802         cpia2_do_command(cam, CPIA2_CMD_GET_VC_CONTROL, TRANSFER_READ, 0);
803         if (cam->params.compression.inhibit_htables) {
804                 tmp_reg = cam->params.vc_params.vc_control |
805                           CPIA2_VC_VC_CTRL_INHIBIT_H_TABLES;
806         } else  {
807                 tmp_reg = cam->params.vc_params.vc_control &
808                           ~CPIA2_VC_VC_CTRL_INHIBIT_H_TABLES;
809         }
810         cpia2_do_command(cam, CPIA2_CMD_SET_VC_CONTROL, TRANSFER_WRITE,tmp_reg);
811
812         /* Set target size (kb) on vc */
813         cpia2_do_command(cam, CPIA2_CMD_SET_TARGET_KB,
814                          TRANSFER_WRITE, cam->params.vc_params.target_kb);
815
816         /* Wiggle VC Reset */
817         /***
818          * First read and wait a bit.
819          ***/
820         for (i = 0; i < 50; i++) {
821                 cpia2_do_command(cam, CPIA2_CMD_GET_PW_CONTROL,
822                                  TRANSFER_READ, 0);
823         }
824
825         tmp_reg = cam->params.vc_params.pw_control;
826         tmp_reg &= ~CPIA2_VC_PW_CTRL_VC_RESET_N;
827
828         cpia2_do_command(cam, CPIA2_CMD_SET_PW_CONTROL, TRANSFER_WRITE,tmp_reg);
829
830         tmp_reg |= CPIA2_VC_PW_CTRL_VC_RESET_N;
831         cpia2_do_command(cam, CPIA2_CMD_SET_PW_CONTROL, TRANSFER_WRITE,tmp_reg);
832
833         cpia2_do_command(cam, CPIA2_CMD_SET_DEF_JPEG_OPT, TRANSFER_WRITE, 0);
834
835         cpia2_do_command(cam, CPIA2_CMD_GET_USER_MODE, TRANSFER_READ, 0);
836         DBG("After VC RESET, user mode is 0x%0X\n",
837             cam->params.vp_params.video_mode);
838
839         return retval;
840 }
841
842 /******************************************************************************
843  *
844  *  cpia2_set_high_power
845  *
846  *****************************************************************************/
847 static int cpia2_set_high_power(struct camera_data *cam)
848 {
849         int i;
850         for (i = 0; i <= 50; i++) {
851                 /* Read system status */
852                 cpia2_do_command(cam,CPIA2_CMD_GET_SYSTEM_CTRL,TRANSFER_READ,0);
853
854                 /* If there is an error, clear it */
855                 if(cam->params.camera_state.system_ctrl &
856                    CPIA2_SYSTEM_CONTROL_V2W_ERR)
857                         cpia2_do_command(cam, CPIA2_CMD_CLEAR_V2W_ERR,
858                                          TRANSFER_WRITE, 0);
859
860                 /* Try to set high power mode */
861                 cpia2_do_command(cam, CPIA2_CMD_SET_SYSTEM_CTRL,
862                                  TRANSFER_WRITE, 1);
863
864                 /* Try to read something in VP to check if everything is awake */
865                 cpia2_do_command(cam, CPIA2_CMD_GET_VP_SYSTEM_STATE,
866                                  TRANSFER_READ, 0);
867                 if (cam->params.vp_params.system_state &
868                     CPIA2_VP_SYSTEMSTATE_HK_ALIVE) {
869                         break;
870                 } else if (i == 50) {
871                         cam->params.camera_state.power_mode = LO_POWER_MODE;
872                         ERR("Camera did not wake up\n");
873                         return -EIO;
874                 }
875         }
876
877         DBG("System now in high power state\n");
878         cam->params.camera_state.power_mode = HI_POWER_MODE;
879         return 0;
880 }
881
882 /******************************************************************************
883  *
884  *  cpia2_set_low_power
885  *
886  *****************************************************************************/
887 int cpia2_set_low_power(struct camera_data *cam)
888 {
889         cam->params.camera_state.power_mode = LO_POWER_MODE;
890         cpia2_do_command(cam, CPIA2_CMD_SET_SYSTEM_CTRL, TRANSFER_WRITE, 0);
891         return 0;
892 }
893
894 /******************************************************************************
895  *
896  *  apply_vp_patch
897  *
898  *****************************************************************************/
899 static int apply_vp_patch(struct camera_data *cam)
900 {
901         int i, j;
902         struct cpia2_command cmd;
903
904         cmd.req_mode = CAMERAACCESS_TYPE_REPEAT | CAMERAACCESS_VP;
905         cmd.direction = TRANSFER_WRITE;
906
907         for (i = 0; i < PATCH_DATA_SIZE; i++) {
908                 for (j = 0; j < patch_data[i].count; j++) {
909                         cmd.buffer.block_data[j] = patch_data[i].data[j];
910                 }
911
912                 cmd.start = patch_data[i].reg;
913                 cmd.reg_count = patch_data[i].count;
914                 cpia2_send_command(cam, &cmd);
915         }
916
917         return 0;
918 }
919
920 /******************************************************************************
921  *
922  *  set_default_user_mode
923  *
924  *****************************************************************************/
925 static int set_default_user_mode(struct camera_data *cam)
926 {
927         unsigned char user_mode;
928         unsigned char frame_rate;
929         int width = cam->params.roi.width;
930         int height = cam->params.roi.height;
931
932         switch (cam->params.version.sensor_flags) {
933         case CPIA2_VP_SENSOR_FLAGS_404:
934         case CPIA2_VP_SENSOR_FLAGS_407:
935         case CPIA2_VP_SENSOR_FLAGS_409:
936         case CPIA2_VP_SENSOR_FLAGS_410:
937                 if ((width > STV_IMAGE_QCIF_COLS)
938                     || (height > STV_IMAGE_QCIF_ROWS)) {
939                         user_mode = CPIA2_VP_USER_MODE_CIF;
940                 } else {
941                         user_mode = CPIA2_VP_USER_MODE_QCIFDS;
942                 }
943                 frame_rate = CPIA2_VP_FRAMERATE_30;
944                 break;
945         case CPIA2_VP_SENSOR_FLAGS_500:
946                 if ((width > STV_IMAGE_CIF_COLS)
947                     || (height > STV_IMAGE_CIF_ROWS)) {
948                         user_mode = CPIA2_VP_USER_MODE_VGA;
949                 } else {
950                         user_mode = CPIA2_VP_USER_MODE_QVGADS;
951                 }
952                 if (cam->params.pnp_id.device_type == DEVICE_STV_672)
953                         frame_rate = CPIA2_VP_FRAMERATE_15;
954                 else
955                         frame_rate = CPIA2_VP_FRAMERATE_30;
956                 break;
957         default:
958                 LOG("%s: Invalid sensor flag value 0x%0X\n",__FUNCTION__,
959                     cam->params.version.sensor_flags);
960                 return -EINVAL;
961         }
962
963         DBG("Sensor flag = 0x%0x, user mode = 0x%0x, frame rate = 0x%X\n",
964             cam->params.version.sensor_flags, user_mode, frame_rate);
965         cpia2_do_command(cam, CPIA2_CMD_SET_USER_MODE, TRANSFER_WRITE,
966                          user_mode);
967         if(cam->params.vp_params.frame_rate > 0 &&
968            frame_rate > cam->params.vp_params.frame_rate)
969                 frame_rate = cam->params.vp_params.frame_rate;
970
971         cpia2_set_fps(cam, frame_rate);
972
973 //      if (cam->params.pnp_id.device_type == DEVICE_STV_676)
974 //              cpia2_do_command(cam,
975 //                               CPIA2_CMD_SET_VP_SYSTEM_CTRL,
976 //                               TRANSFER_WRITE,
977 //                               CPIA2_VP_SYSTEMCTRL_HK_CONTROL |
978 //                               CPIA2_VP_SYSTEMCTRL_POWER_CONTROL);
979
980         return 0;
981 }
982
983 /******************************************************************************
984  *
985  *  cpia2_match_video_size
986  *
987  *  return the best match, where 'best' is as always
988  *  the largest that is not bigger than what is requested.
989  *****************************************************************************/
990 int cpia2_match_video_size(int width, int height)
991 {
992         if (width >= STV_IMAGE_VGA_COLS && height >= STV_IMAGE_VGA_ROWS)
993                 return VIDEOSIZE_VGA;
994
995         if (width >= STV_IMAGE_CIF_COLS && height >= STV_IMAGE_CIF_ROWS)
996                 return VIDEOSIZE_CIF;
997
998         if (width >= STV_IMAGE_QVGA_COLS && height >= STV_IMAGE_QVGA_ROWS)
999                 return VIDEOSIZE_QVGA;
1000
1001         if (width >= 288 && height >= 216)
1002                 return VIDEOSIZE_288_216;
1003
1004         if (width >= 256 && height >= 192)
1005                 return VIDEOSIZE_256_192;
1006
1007         if (width >= 224 && height >= 168)
1008                 return VIDEOSIZE_224_168;
1009
1010         if (width >= 192 && height >= 144)
1011                 return VIDEOSIZE_192_144;
1012
1013         if (width >= STV_IMAGE_QCIF_COLS && height >= STV_IMAGE_QCIF_ROWS)
1014                 return VIDEOSIZE_QCIF;
1015
1016         return -1;
1017 }
1018
1019 /******************************************************************************
1020  *
1021  *  SetVideoSize
1022  *
1023  *****************************************************************************/
1024 static int set_vw_size(struct camera_data *cam, int size)
1025 {
1026         int retval = 0;
1027
1028         cam->params.vp_params.video_size = size;
1029
1030         switch (size) {
1031         case VIDEOSIZE_VGA:
1032                 DBG("Setting size to VGA\n");
1033                 cam->params.roi.width = STV_IMAGE_VGA_COLS;
1034                 cam->params.roi.height = STV_IMAGE_VGA_ROWS;
1035                 cam->vw.width = STV_IMAGE_VGA_COLS;
1036                 cam->vw.height = STV_IMAGE_VGA_ROWS;
1037                 break;
1038         case VIDEOSIZE_CIF:
1039                 DBG("Setting size to CIF\n");
1040                 cam->params.roi.width = STV_IMAGE_CIF_COLS;
1041                 cam->params.roi.height = STV_IMAGE_CIF_ROWS;
1042                 cam->vw.width = STV_IMAGE_CIF_COLS;
1043                 cam->vw.height = STV_IMAGE_CIF_ROWS;
1044                 break;
1045         case VIDEOSIZE_QVGA:
1046                 DBG("Setting size to QVGA\n");
1047                 cam->params.roi.width = STV_IMAGE_QVGA_COLS;
1048                 cam->params.roi.height = STV_IMAGE_QVGA_ROWS;
1049                 cam->vw.width = STV_IMAGE_QVGA_COLS;
1050                 cam->vw.height = STV_IMAGE_QVGA_ROWS;
1051                 break;
1052         case VIDEOSIZE_288_216:
1053                 cam->params.roi.width = 288;
1054                 cam->params.roi.height = 216;
1055                 cam->vw.width = 288;
1056                 cam->vw.height = 216;
1057                 break;
1058         case VIDEOSIZE_256_192:
1059                 cam->vw.width = 256;
1060                 cam->vw.height = 192;
1061                 cam->params.roi.width = 256;
1062                 cam->params.roi.height = 192;
1063                 break;
1064         case VIDEOSIZE_224_168:
1065                 cam->vw.width = 224;
1066                 cam->vw.height = 168;
1067                 cam->params.roi.width = 224;
1068                 cam->params.roi.height = 168;
1069                 break;
1070         case VIDEOSIZE_192_144:
1071                 cam->vw.width = 192;
1072                 cam->vw.height = 144;
1073                 cam->params.roi.width = 192;
1074                 cam->params.roi.height = 144;
1075                 break;
1076         case VIDEOSIZE_QCIF:
1077                 DBG("Setting size to QCIF\n");
1078                 cam->params.roi.width = STV_IMAGE_QCIF_COLS;
1079                 cam->params.roi.height = STV_IMAGE_QCIF_ROWS;
1080                 cam->vw.width = STV_IMAGE_QCIF_COLS;
1081                 cam->vw.height = STV_IMAGE_QCIF_ROWS;
1082                 break;
1083         default:
1084                 retval = -EINVAL;
1085         }
1086         return retval;
1087 }
1088
1089 /******************************************************************************
1090  *
1091  *  configure_sensor
1092  *
1093  *****************************************************************************/
1094 static int configure_sensor(struct camera_data *cam,
1095                             int req_width, int req_height)
1096 {
1097         int retval;
1098
1099         switch (cam->params.version.sensor_flags) {
1100         case CPIA2_VP_SENSOR_FLAGS_404:
1101         case CPIA2_VP_SENSOR_FLAGS_407:
1102         case CPIA2_VP_SENSOR_FLAGS_409:
1103         case CPIA2_VP_SENSOR_FLAGS_410:
1104                 retval = config_sensor_410(cam, req_width, req_height);
1105                 break;
1106         case CPIA2_VP_SENSOR_FLAGS_500:
1107                 retval = config_sensor_500(cam, req_width, req_height);
1108                 break;
1109         default:
1110                 return -EINVAL;
1111         }
1112
1113         return retval;
1114 }
1115
1116 /******************************************************************************
1117  *
1118  *  config_sensor_410
1119  *
1120  *****************************************************************************/
1121 static int config_sensor_410(struct camera_data *cam,
1122                             int req_width, int req_height)
1123 {
1124         struct cpia2_command cmd;
1125         int i = 0;
1126         int image_size;
1127         int image_type;
1128         int width = req_width;
1129         int height = req_height;
1130
1131         /***
1132          *  Make sure size doesn't exceed CIF.
1133          ***/
1134         if (width > STV_IMAGE_CIF_COLS)
1135                 width = STV_IMAGE_CIF_COLS;
1136         if (height > STV_IMAGE_CIF_ROWS)
1137                 height = STV_IMAGE_CIF_ROWS;
1138
1139         image_size = cpia2_match_video_size(width, height);
1140
1141         DBG("Config 410: width = %d, height = %d\n", width, height);
1142         DBG("Image size returned is %d\n", image_size);
1143         if (image_size >= 0) {
1144                 set_vw_size(cam, image_size);
1145                 width = cam->params.roi.width;
1146                 height = cam->params.roi.height;
1147
1148                 DBG("After set_vw_size(), width = %d, height = %d\n",
1149                     width, height);
1150                 if (width <= 176 && height <= 144) {
1151                         DBG("image type = VIDEOSIZE_QCIF\n");
1152                         image_type = VIDEOSIZE_QCIF;
1153                 }
1154                 else if (width <= 320 && height <= 240) {
1155                         DBG("image type = VIDEOSIZE_QVGA\n");
1156                         image_type = VIDEOSIZE_QVGA;
1157                 }
1158                 else {
1159                         DBG("image type = VIDEOSIZE_CIF\n");
1160                         image_type = VIDEOSIZE_CIF;
1161                 }
1162         } else {
1163                 ERR("ConfigSensor410 failed\n");
1164                 return -EINVAL;
1165         }
1166
1167         cmd.req_mode = CAMERAACCESS_TYPE_RANDOM | CAMERAACCESS_VC;
1168         cmd.direction = TRANSFER_WRITE;
1169
1170         /* VC Format */
1171         cmd.buffer.registers[i].index = CPIA2_VC_VC_FORMAT;
1172         if (image_type == VIDEOSIZE_CIF) {
1173                 cmd.buffer.registers[i++].value =
1174                     (u8) (CPIA2_VC_VC_FORMAT_UFIRST |
1175                           CPIA2_VC_VC_FORMAT_SHORTLINE);
1176         } else {
1177                 cmd.buffer.registers[i++].value =
1178                     (u8) CPIA2_VC_VC_FORMAT_UFIRST;
1179         }
1180
1181         /* VC Clocks */
1182         cmd.buffer.registers[i].index = CPIA2_VC_VC_CLOCKS;
1183         if (image_type == VIDEOSIZE_QCIF) {
1184                 if (cam->params.pnp_id.device_type == DEVICE_STV_672) {
1185                         cmd.buffer.registers[i++].value=
1186                                 (u8)(CPIA2_VC_VC_672_CLOCKS_CIF_DIV_BY_3 |
1187                                      CPIA2_VC_VC_672_CLOCKS_SCALING |
1188                                      CPIA2_VC_VC_CLOCKS_LOGDIV2);
1189                         DBG("VC_Clocks (0xc4) should be B\n");
1190                 }
1191                 else {
1192                         cmd.buffer.registers[i++].value=
1193                                 (u8)(CPIA2_VC_VC_676_CLOCKS_CIF_DIV_BY_3 |
1194                                      CPIA2_VC_VC_CLOCKS_LOGDIV2);
1195                 }
1196         } else {
1197                 if (cam->params.pnp_id.device_type == DEVICE_STV_672) {
1198                         cmd.buffer.registers[i++].value =
1199                            (u8) (CPIA2_VC_VC_672_CLOCKS_CIF_DIV_BY_3 |
1200                                  CPIA2_VC_VC_CLOCKS_LOGDIV0);
1201                 }
1202                 else {
1203                         cmd.buffer.registers[i++].value =
1204                            (u8) (CPIA2_VC_VC_676_CLOCKS_CIF_DIV_BY_3 |
1205                                  CPIA2_VC_VC_676_CLOCKS_SCALING |
1206                                  CPIA2_VC_VC_CLOCKS_LOGDIV0);
1207                 }
1208         }
1209         DBG("VC_Clocks (0xc4) = 0x%0X\n", cmd.buffer.registers[i-1].value);
1210
1211         /* Input reqWidth from VC */
1212         cmd.buffer.registers[i].index = CPIA2_VC_VC_IHSIZE_LO;
1213         if (image_type == VIDEOSIZE_QCIF)
1214                 cmd.buffer.registers[i++].value =
1215                     (u8) (STV_IMAGE_QCIF_COLS / 4);
1216         else
1217                 cmd.buffer.registers[i++].value =
1218                     (u8) (STV_IMAGE_CIF_COLS / 4);
1219
1220         /* Timings */
1221         cmd.buffer.registers[i].index = CPIA2_VC_VC_XLIM_HI;
1222         if (image_type == VIDEOSIZE_QCIF)
1223                 cmd.buffer.registers[i++].value = (u8) 0;
1224         else
1225                 cmd.buffer.registers[i++].value = (u8) 1;
1226
1227         cmd.buffer.registers[i].index = CPIA2_VC_VC_XLIM_LO;
1228         if (image_type == VIDEOSIZE_QCIF)
1229                 cmd.buffer.registers[i++].value = (u8) 208;
1230         else
1231                 cmd.buffer.registers[i++].value = (u8) 160;
1232
1233         cmd.buffer.registers[i].index = CPIA2_VC_VC_YLIM_HI;
1234         if (image_type == VIDEOSIZE_QCIF)
1235                 cmd.buffer.registers[i++].value = (u8) 0;
1236         else
1237                 cmd.buffer.registers[i++].value = (u8) 1;
1238
1239         cmd.buffer.registers[i].index = CPIA2_VC_VC_YLIM_LO;
1240         if (image_type == VIDEOSIZE_QCIF)
1241                 cmd.buffer.registers[i++].value = (u8) 160;
1242         else
1243                 cmd.buffer.registers[i++].value = (u8) 64;
1244
1245         /* Output Image Size */
1246         cmd.buffer.registers[i].index = CPIA2_VC_VC_OHSIZE;
1247         cmd.buffer.registers[i++].value = cam->params.roi.width / 4;
1248
1249         cmd.buffer.registers[i].index = CPIA2_VC_VC_OVSIZE;
1250         cmd.buffer.registers[i++].value = cam->params.roi.height / 4;
1251
1252         /* Cropping */
1253         cmd.buffer.registers[i].index = CPIA2_VC_VC_HCROP;
1254         if (image_type == VIDEOSIZE_QCIF)
1255                 cmd.buffer.registers[i++].value =
1256                     (u8) (((STV_IMAGE_QCIF_COLS / 4) - (width / 4)) / 2);
1257         else
1258                 cmd.buffer.registers[i++].value =
1259                     (u8) (((STV_IMAGE_CIF_COLS / 4) - (width / 4)) / 2);
1260
1261         cmd.buffer.registers[i].index = CPIA2_VC_VC_VCROP;
1262         if (image_type == VIDEOSIZE_QCIF)
1263                 cmd.buffer.registers[i++].value =
1264                     (u8) (((STV_IMAGE_QCIF_ROWS / 4) - (height / 4)) / 2);
1265         else
1266                 cmd.buffer.registers[i++].value =
1267                     (u8) (((STV_IMAGE_CIF_ROWS / 4) - (height / 4)) / 2);
1268
1269         /* Scaling registers (defaults) */
1270         cmd.buffer.registers[i].index = CPIA2_VC_VC_HPHASE;
1271         cmd.buffer.registers[i++].value = (u8) 0;
1272
1273         cmd.buffer.registers[i].index = CPIA2_VC_VC_VPHASE;
1274         cmd.buffer.registers[i++].value = (u8) 0;
1275
1276         cmd.buffer.registers[i].index = CPIA2_VC_VC_HISPAN;
1277         cmd.buffer.registers[i++].value = (u8) 31;
1278
1279         cmd.buffer.registers[i].index = CPIA2_VC_VC_VISPAN;
1280         cmd.buffer.registers[i++].value = (u8) 31;
1281
1282         cmd.buffer.registers[i].index = CPIA2_VC_VC_HICROP;
1283         cmd.buffer.registers[i++].value = (u8) 0;
1284
1285         cmd.buffer.registers[i].index = CPIA2_VC_VC_VICROP;
1286         cmd.buffer.registers[i++].value = (u8) 0;
1287
1288         cmd.buffer.registers[i].index = CPIA2_VC_VC_HFRACT;
1289         cmd.buffer.registers[i++].value = (u8) 0x81;    /* = 8/1 = 8 (HIBYTE/LOBYTE) */
1290
1291         cmd.buffer.registers[i].index = CPIA2_VC_VC_VFRACT;
1292         cmd.buffer.registers[i++].value = (u8) 0x81;    /* = 8/1 = 8 (HIBYTE/LOBYTE) */
1293
1294         cmd.reg_count = i;
1295
1296         cpia2_send_command(cam, &cmd);
1297
1298         return i;
1299 }
1300
1301
1302 /******************************************************************************
1303  *
1304  *  config_sensor_500(cam)
1305  *
1306  *****************************************************************************/
1307 static int config_sensor_500(struct camera_data *cam,
1308                              int req_width, int req_height)
1309 {
1310         struct cpia2_command cmd;
1311         int i = 0;
1312         int image_size = VIDEOSIZE_CIF;
1313         int image_type = VIDEOSIZE_VGA;
1314         int width = req_width;
1315         int height = req_height;
1316         unsigned int device = cam->params.pnp_id.device_type;
1317
1318         image_size = cpia2_match_video_size(width, height);
1319
1320         if (width > STV_IMAGE_CIF_COLS || height > STV_IMAGE_CIF_ROWS)
1321                 image_type = VIDEOSIZE_VGA;
1322         else if (width > STV_IMAGE_QVGA_COLS || height > STV_IMAGE_QVGA_ROWS)
1323                 image_type = VIDEOSIZE_CIF;
1324         else if (width > STV_IMAGE_QCIF_COLS || height > STV_IMAGE_QCIF_ROWS)
1325                 image_type = VIDEOSIZE_QVGA;
1326         else
1327                 image_type = VIDEOSIZE_QCIF;
1328
1329         if (image_size >= 0) {
1330                 set_vw_size(cam, image_size);
1331                 width = cam->params.roi.width;
1332                 height = cam->params.roi.height;
1333         } else {
1334                 ERR("ConfigSensor500 failed\n");
1335                 return -EINVAL;
1336         }
1337
1338         DBG("image_size = %d, width = %d, height = %d, type = %d\n",
1339             image_size, width, height, image_type);
1340
1341         cmd.req_mode = CAMERAACCESS_TYPE_RANDOM | CAMERAACCESS_VC;
1342         cmd.direction = TRANSFER_WRITE;
1343         i = 0;
1344
1345         /* VC Format */
1346         cmd.buffer.registers[i].index = CPIA2_VC_VC_FORMAT;
1347         cmd.buffer.registers[i].value = (u8) CPIA2_VC_VC_FORMAT_UFIRST;
1348         if (image_type == VIDEOSIZE_QCIF)
1349                 cmd.buffer.registers[i].value |= (u8) CPIA2_VC_VC_FORMAT_DECIMATING;
1350         i++;
1351
1352         /* VC Clocks */
1353         cmd.buffer.registers[i].index = CPIA2_VC_VC_CLOCKS;
1354         if (device == DEVICE_STV_672) {
1355                 if (image_type == VIDEOSIZE_VGA)
1356                         cmd.buffer.registers[i].value =
1357                                 (u8)CPIA2_VC_VC_CLOCKS_LOGDIV1;
1358                 else
1359                         cmd.buffer.registers[i].value =
1360                                 (u8)(CPIA2_VC_VC_672_CLOCKS_SCALING |
1361                                      CPIA2_VC_VC_CLOCKS_LOGDIV3);
1362         } else {
1363                 if (image_type == VIDEOSIZE_VGA)
1364                         cmd.buffer.registers[i].value =
1365                                 (u8)CPIA2_VC_VC_CLOCKS_LOGDIV0;
1366                 else
1367                         cmd.buffer.registers[i].value =
1368                                 (u8)(CPIA2_VC_VC_676_CLOCKS_SCALING |
1369                                      CPIA2_VC_VC_CLOCKS_LOGDIV2);
1370         }
1371         i++;
1372
1373         DBG("VC_CLOCKS = 0x%X\n", cmd.buffer.registers[i-1].value);
1374
1375         /* Input width from VP */
1376         cmd.buffer.registers[i].index = CPIA2_VC_VC_IHSIZE_LO;
1377         if (image_type == VIDEOSIZE_VGA)
1378                 cmd.buffer.registers[i].value =
1379                     (u8) (STV_IMAGE_VGA_COLS / 4);
1380         else
1381                 cmd.buffer.registers[i].value =
1382                     (u8) (STV_IMAGE_QVGA_COLS / 4);
1383         i++;
1384         DBG("Input width = %d\n", cmd.buffer.registers[i-1].value);
1385
1386         /* Timings */
1387         cmd.buffer.registers[i].index = CPIA2_VC_VC_XLIM_HI;
1388         if (image_type == VIDEOSIZE_VGA)
1389                 cmd.buffer.registers[i++].value = (u8) 2;
1390         else
1391                 cmd.buffer.registers[i++].value = (u8) 1;
1392
1393         cmd.buffer.registers[i].index = CPIA2_VC_VC_XLIM_LO;
1394         if (image_type == VIDEOSIZE_VGA)
1395                 cmd.buffer.registers[i++].value = (u8) 250;
1396         else if (image_type == VIDEOSIZE_QVGA)
1397                 cmd.buffer.registers[i++].value = (u8) 125;
1398         else
1399                 cmd.buffer.registers[i++].value = (u8) 160;
1400
1401         cmd.buffer.registers[i].index = CPIA2_VC_VC_YLIM_HI;
1402         if (image_type == VIDEOSIZE_VGA)
1403                 cmd.buffer.registers[i++].value = (u8) 2;
1404         else
1405                 cmd.buffer.registers[i++].value = (u8) 1;
1406
1407         cmd.buffer.registers[i].index = CPIA2_VC_VC_YLIM_LO;
1408         if (image_type == VIDEOSIZE_VGA)
1409                 cmd.buffer.registers[i++].value = (u8) 12;
1410         else if (image_type == VIDEOSIZE_QVGA)
1411                 cmd.buffer.registers[i++].value = (u8) 64;
1412         else
1413                 cmd.buffer.registers[i++].value = (u8) 6;
1414
1415         /* Output Image Size */
1416         cmd.buffer.registers[i].index = CPIA2_VC_VC_OHSIZE;
1417         if (image_type == VIDEOSIZE_QCIF)
1418                 cmd.buffer.registers[i++].value = STV_IMAGE_CIF_COLS  / 4;
1419         else
1420                 cmd.buffer.registers[i++].value = width / 4;
1421
1422         cmd.buffer.registers[i].index = CPIA2_VC_VC_OVSIZE;
1423         if (image_type == VIDEOSIZE_QCIF)
1424                 cmd.buffer.registers[i++].value = STV_IMAGE_CIF_ROWS  / 4;
1425         else
1426                 cmd.buffer.registers[i++].value = height / 4;
1427
1428         /* Cropping */
1429         cmd.buffer.registers[i].index = CPIA2_VC_VC_HCROP;
1430         if (image_type == VIDEOSIZE_VGA)
1431                 cmd.buffer.registers[i++].value =
1432                     (u8) (((STV_IMAGE_VGA_COLS / 4) - (width / 4)) / 2);
1433         else if (image_type == VIDEOSIZE_QVGA)
1434                 cmd.buffer.registers[i++].value =
1435                     (u8) (((STV_IMAGE_QVGA_COLS / 4) - (width / 4)) / 2);
1436         else if (image_type == VIDEOSIZE_CIF)
1437                 cmd.buffer.registers[i++].value =
1438                     (u8) (((STV_IMAGE_CIF_COLS / 4) - (width / 4)) / 2);
1439         else /*if (image_type == VIDEOSIZE_QCIF)*/
1440                 cmd.buffer.registers[i++].value =
1441                         (u8) (((STV_IMAGE_QCIF_COLS / 4) - (width / 4)) / 2);
1442
1443         cmd.buffer.registers[i].index = CPIA2_VC_VC_VCROP;
1444         if (image_type == VIDEOSIZE_VGA)
1445                 cmd.buffer.registers[i++].value =
1446                     (u8) (((STV_IMAGE_VGA_ROWS / 4) - (height / 4)) / 2);
1447         else if (image_type == VIDEOSIZE_QVGA)
1448                 cmd.buffer.registers[i++].value =
1449                     (u8) (((STV_IMAGE_QVGA_ROWS / 4) - (height / 4)) / 2);
1450         else if (image_type == VIDEOSIZE_CIF)
1451                 cmd.buffer.registers[i++].value =
1452                     (u8) (((STV_IMAGE_CIF_ROWS / 4) - (height / 4)) / 2);
1453         else /*if (image_type == VIDEOSIZE_QCIF)*/
1454                 cmd.buffer.registers[i++].value =
1455                     (u8) (((STV_IMAGE_QCIF_ROWS / 4) - (height / 4)) / 2);
1456
1457         /* Scaling registers (defaults) */
1458         cmd.buffer.registers[i].index = CPIA2_VC_VC_HPHASE;
1459         if (image_type == VIDEOSIZE_CIF || image_type == VIDEOSIZE_QCIF)
1460                 cmd.buffer.registers[i++].value = (u8) 36;
1461         else
1462                 cmd.buffer.registers[i++].value = (u8) 0;
1463
1464         cmd.buffer.registers[i].index = CPIA2_VC_VC_VPHASE;
1465         if (image_type == VIDEOSIZE_CIF || image_type == VIDEOSIZE_QCIF)
1466                 cmd.buffer.registers[i++].value = (u8) 32;
1467         else
1468                 cmd.buffer.registers[i++].value = (u8) 0;
1469
1470         cmd.buffer.registers[i].index = CPIA2_VC_VC_HISPAN;
1471         if (image_type == VIDEOSIZE_CIF || image_type == VIDEOSIZE_QCIF)
1472                 cmd.buffer.registers[i++].value = (u8) 26;
1473         else
1474                 cmd.buffer.registers[i++].value = (u8) 31;
1475
1476         cmd.buffer.registers[i].index = CPIA2_VC_VC_VISPAN;
1477         if (image_type == VIDEOSIZE_CIF || image_type == VIDEOSIZE_QCIF)
1478                 cmd.buffer.registers[i++].value = (u8) 21;
1479         else
1480                 cmd.buffer.registers[i++].value = (u8) 31;
1481
1482         cmd.buffer.registers[i].index = CPIA2_VC_VC_HICROP;
1483         cmd.buffer.registers[i++].value = (u8) 0;
1484
1485         cmd.buffer.registers[i].index = CPIA2_VC_VC_VICROP;
1486         cmd.buffer.registers[i++].value = (u8) 0;
1487
1488         cmd.buffer.registers[i].index = CPIA2_VC_VC_HFRACT;
1489         if (image_type == VIDEOSIZE_CIF || image_type == VIDEOSIZE_QCIF)
1490                 cmd.buffer.registers[i++].value = (u8) 0x2B;    /* 2/11 */
1491         else
1492                 cmd.buffer.registers[i++].value = (u8) 0x81;    /* 8/1 */
1493
1494         cmd.buffer.registers[i].index = CPIA2_VC_VC_VFRACT;
1495         if (image_type == VIDEOSIZE_CIF || image_type == VIDEOSIZE_QCIF)
1496                 cmd.buffer.registers[i++].value = (u8) 0x13;    /* 1/3 */
1497         else
1498                 cmd.buffer.registers[i++].value = (u8) 0x81;    /* 8/1 */
1499
1500         cmd.reg_count = i;
1501
1502         cpia2_send_command(cam, &cmd);
1503
1504         return i;
1505 }
1506
1507
1508 /******************************************************************************
1509  *
1510  *  setallproperties
1511  *
1512  *  This sets all user changeable properties to the values in cam->params.
1513  *****************************************************************************/
1514 int set_all_properties(struct camera_data *cam)
1515 {
1516         /**
1517          * Don't set target_kb here, it will be set later.
1518          * framerate and user_mode were already set (set_default_user_mode).
1519          **/
1520
1521         cpia2_set_color_params(cam);
1522
1523         cpia2_usb_change_streaming_alternate(cam,
1524                                           cam->params.camera_state.stream_mode);
1525
1526         cpia2_do_command(cam, CPIA2_CMD_SET_USER_EFFECTS, TRANSFER_WRITE,
1527                          cam->params.vp_params.user_effects);
1528
1529         cpia2_set_flicker_mode(cam,
1530                                cam->params.flicker_control.flicker_mode_req);
1531
1532         cpia2_do_command(cam,
1533                          CPIA2_CMD_SET_VC_MP_GPIO_DIRECTION,
1534                          TRANSFER_WRITE, cam->params.vp_params.gpio_direction);
1535         cpia2_do_command(cam, CPIA2_CMD_SET_VC_MP_GPIO_DATA, TRANSFER_WRITE,
1536                          cam->params.vp_params.gpio_data);
1537
1538         wake_system(cam);
1539
1540         set_lowlight_boost(cam);
1541
1542         return 0;
1543 }
1544
1545 /******************************************************************************
1546  *
1547  *  cpia2_save_camera_state
1548  *
1549  *****************************************************************************/
1550 void cpia2_save_camera_state(struct camera_data *cam)
1551 {
1552         get_color_params(cam);
1553         cpia2_do_command(cam, CPIA2_CMD_GET_USER_EFFECTS, TRANSFER_READ, 0);
1554         cpia2_do_command(cam, CPIA2_CMD_GET_VC_MP_GPIO_DIRECTION, TRANSFER_READ,
1555                          0);
1556         cpia2_do_command(cam, CPIA2_CMD_GET_VC_MP_GPIO_DATA, TRANSFER_READ, 0);
1557         /* Don't get framerate or target_kb. Trust the values we already have */
1558 }
1559
1560 /******************************************************************************
1561  *
1562  *  get_color_params
1563  *
1564  *****************************************************************************/
1565 void get_color_params(struct camera_data *cam)
1566 {
1567         cpia2_do_command(cam, CPIA2_CMD_GET_VP_BRIGHTNESS, TRANSFER_READ, 0);
1568         cpia2_do_command(cam, CPIA2_CMD_GET_VP_SATURATION, TRANSFER_READ, 0);
1569         cpia2_do_command(cam, CPIA2_CMD_GET_CONTRAST, TRANSFER_READ, 0);
1570 }
1571
1572 /******************************************************************************
1573  *
1574  *  cpia2_set_color_params
1575  *
1576  *****************************************************************************/
1577 void cpia2_set_color_params(struct camera_data *cam)
1578 {
1579         DBG("Setting color params\n");
1580         cpia2_set_brightness(cam, cam->params.color_params.brightness);
1581         cpia2_set_contrast(cam, cam->params.color_params.contrast);
1582         cpia2_set_saturation(cam, cam->params.color_params.saturation);
1583 }
1584
1585 /******************************************************************************
1586  *
1587  *  cpia2_set_flicker_mode
1588  *
1589  *****************************************************************************/
1590 int cpia2_set_flicker_mode(struct camera_data *cam, int mode)
1591 {
1592         unsigned char cam_reg;
1593         int err = 0;
1594
1595         if(cam->params.pnp_id.device_type != DEVICE_STV_672)
1596                 return -EINVAL;
1597
1598         /* Set the appropriate bits in FLICKER_MODES, preserving the rest */
1599         if((err = cpia2_do_command(cam, CPIA2_CMD_GET_FLICKER_MODES,
1600                                    TRANSFER_READ, 0)))
1601                 return err;
1602         cam_reg = cam->params.flicker_control.cam_register;
1603
1604         switch(mode) {
1605         case NEVER_FLICKER:
1606                 cam_reg |= CPIA2_VP_FLICKER_MODES_NEVER_FLICKER;
1607                 cam_reg &= ~CPIA2_VP_FLICKER_MODES_50HZ;
1608                 break;
1609         case FLICKER_60:
1610                 cam_reg &= ~CPIA2_VP_FLICKER_MODES_NEVER_FLICKER;
1611                 cam_reg &= ~CPIA2_VP_FLICKER_MODES_50HZ;
1612                 break;
1613         case FLICKER_50:
1614                 cam_reg &= ~CPIA2_VP_FLICKER_MODES_NEVER_FLICKER;
1615                 cam_reg |= CPIA2_VP_FLICKER_MODES_50HZ;
1616                 break;
1617         default:
1618                 return -EINVAL;
1619         }
1620
1621         if((err = cpia2_do_command(cam, CPIA2_CMD_SET_FLICKER_MODES,
1622                                    TRANSFER_WRITE, cam_reg)))
1623                 return err;
1624
1625         /* Set the appropriate bits in EXP_MODES, preserving the rest */
1626         if((err = cpia2_do_command(cam, CPIA2_CMD_GET_VP_EXP_MODES,
1627                                    TRANSFER_READ, 0)))
1628                 return err;
1629         cam_reg = cam->params.vp_params.exposure_modes;
1630
1631         if (mode == NEVER_FLICKER) {
1632                 cam_reg |= CPIA2_VP_EXPOSURE_MODES_INHIBIT_FLICKER;
1633         } else {
1634                 cam_reg &= ~CPIA2_VP_EXPOSURE_MODES_INHIBIT_FLICKER;
1635         }
1636
1637         if((err = cpia2_do_command(cam, CPIA2_CMD_SET_VP_EXP_MODES,
1638                                    TRANSFER_WRITE, cam_reg)))
1639                 return err;
1640
1641         if((err = cpia2_do_command(cam, CPIA2_CMD_REHASH_VP4,
1642                                    TRANSFER_WRITE, 1)))
1643                 return err;
1644
1645         switch(mode) {
1646         case NEVER_FLICKER:
1647                 cam->params.flicker_control.flicker_mode_req = mode;
1648                 break;
1649         case FLICKER_60:
1650                 cam->params.flicker_control.flicker_mode_req = mode;
1651                 cam->params.flicker_control.mains_frequency = 60;
1652                 break;
1653         case FLICKER_50:
1654                 cam->params.flicker_control.flicker_mode_req = mode;
1655                 cam->params.flicker_control.mains_frequency = 50;
1656                 break;
1657         default:
1658                 err = -EINVAL;
1659         }
1660
1661         return err;
1662 }
1663
1664 /******************************************************************************
1665  *
1666  *  cpia2_set_property_flip
1667  *
1668  *****************************************************************************/
1669 void cpia2_set_property_flip(struct camera_data *cam, int prop_val)
1670 {
1671         unsigned char cam_reg;
1672
1673         cpia2_do_command(cam, CPIA2_CMD_GET_USER_EFFECTS, TRANSFER_READ, 0);
1674         cam_reg = cam->params.vp_params.user_effects;
1675
1676         if (prop_val)
1677         {
1678                 cam_reg |= CPIA2_VP_USER_EFFECTS_FLIP;
1679         }
1680         else
1681         {
1682                 cam_reg &= ~CPIA2_VP_USER_EFFECTS_FLIP;
1683         }
1684         cpia2_do_command(cam, CPIA2_CMD_SET_USER_EFFECTS, TRANSFER_WRITE,
1685                          cam_reg);
1686 }
1687
1688 /******************************************************************************
1689  *
1690  *  cpia2_set_property_mirror
1691  *
1692  *****************************************************************************/
1693 void cpia2_set_property_mirror(struct camera_data *cam, int prop_val)
1694 {
1695         unsigned char cam_reg;
1696
1697         cpia2_do_command(cam, CPIA2_CMD_GET_USER_EFFECTS, TRANSFER_READ, 0);
1698         cam_reg = cam->params.vp_params.user_effects;
1699
1700         if (prop_val)
1701         {
1702                 cam_reg |= CPIA2_VP_USER_EFFECTS_MIRROR;
1703         }
1704         else
1705         {
1706                 cam_reg &= ~CPIA2_VP_USER_EFFECTS_MIRROR;
1707         }
1708         cpia2_do_command(cam, CPIA2_CMD_SET_USER_EFFECTS, TRANSFER_WRITE,
1709                          cam_reg);
1710 }
1711
1712 /******************************************************************************
1713  *
1714  *  set_target_kb
1715  *
1716  *  The new Target KB is set in cam->params.vc_params.target_kb and
1717  *  activates on reset.
1718  *****************************************************************************/
1719
1720 int cpia2_set_target_kb(struct camera_data *cam, unsigned char value)
1721 {
1722         DBG("Requested target_kb = %d\n", value);
1723         if (value != cam->params.vc_params.target_kb) {
1724
1725                 cpia2_usb_stream_pause(cam);
1726
1727                 /* reset camera for new target_kb */
1728                 cam->params.vc_params.target_kb = value;
1729                 cpia2_reset_camera(cam);
1730
1731                 cpia2_usb_stream_resume(cam);
1732         }
1733
1734         return 0;
1735 }
1736
1737 /******************************************************************************
1738  *
1739  *  cpia2_set_gpio
1740  *
1741  *****************************************************************************/
1742 int cpia2_set_gpio(struct camera_data *cam, unsigned char setting)
1743 {
1744         int ret;
1745
1746         /* Set the microport direction (register 0x90, should be defined
1747          * already) to 1 (user output), and set the microport data (0x91) to
1748          * the value in the ioctl argument.
1749          */
1750
1751         ret = cpia2_do_command(cam,
1752                                CPIA2_CMD_SET_VC_MP_GPIO_DIRECTION,
1753                                CPIA2_VC_MP_DIR_OUTPUT,
1754                                255);
1755         if (ret < 0)
1756                 return ret;
1757         cam->params.vp_params.gpio_direction = 255;
1758
1759         ret = cpia2_do_command(cam,
1760                                CPIA2_CMD_SET_VC_MP_GPIO_DATA,
1761                                CPIA2_VC_MP_DIR_OUTPUT,
1762                                setting);
1763         if (ret < 0)
1764                 return ret;
1765         cam->params.vp_params.gpio_data = setting;
1766
1767         return 0;
1768 }
1769
1770 /******************************************************************************
1771  *
1772  *  cpia2_set_fps
1773  *
1774  *****************************************************************************/
1775 int cpia2_set_fps(struct camera_data *cam, int framerate)
1776 {
1777         int retval;
1778
1779         switch(framerate) {
1780                 case CPIA2_VP_FRAMERATE_30:
1781                 case CPIA2_VP_FRAMERATE_25:
1782                         if(cam->params.pnp_id.device_type == DEVICE_STV_672 &&
1783                            cam->params.version.sensor_flags ==
1784                                                     CPIA2_VP_SENSOR_FLAGS_500) {
1785                                 return -EINVAL;
1786                         }
1787                         /* Fall through */
1788                 case CPIA2_VP_FRAMERATE_15:
1789                 case CPIA2_VP_FRAMERATE_12_5:
1790                 case CPIA2_VP_FRAMERATE_7_5:
1791                 case CPIA2_VP_FRAMERATE_6_25:
1792                         break;
1793                 default:
1794                         return -EINVAL;
1795         }
1796
1797         if (cam->params.pnp_id.device_type == DEVICE_STV_672 &&
1798             framerate == CPIA2_VP_FRAMERATE_15)
1799                 framerate = 0; /* Work around bug in VP4 */
1800
1801         retval = cpia2_do_command(cam,
1802                                  CPIA2_CMD_FRAMERATE_REQ,
1803                                  TRANSFER_WRITE,
1804                                  framerate);
1805
1806         if(retval == 0)
1807                 cam->params.vp_params.frame_rate = framerate;
1808
1809         return retval;
1810 }
1811
1812 /******************************************************************************
1813  *
1814  *  cpia2_set_brightness
1815  *
1816  *****************************************************************************/
1817 void cpia2_set_brightness(struct camera_data *cam, unsigned char value)
1818 {
1819         /***
1820          * Don't let the register be set to zero - bug in VP4 - flash of full
1821          * brightness
1822          ***/
1823         if (cam->params.pnp_id.device_type == DEVICE_STV_672 && value == 0)
1824                 value++;
1825         DBG("Setting brightness to %d (0x%0x)\n", value, value);
1826         cpia2_do_command(cam,CPIA2_CMD_SET_VP_BRIGHTNESS, TRANSFER_WRITE,value);
1827 }
1828
1829 /******************************************************************************
1830  *
1831  *  cpia2_set_contrast
1832  *
1833  *****************************************************************************/
1834 void cpia2_set_contrast(struct camera_data *cam, unsigned char value)
1835 {
1836         DBG("Setting contrast to %d (0x%0x)\n", value, value);
1837         cam->params.color_params.contrast = value;
1838         cpia2_do_command(cam, CPIA2_CMD_SET_CONTRAST, TRANSFER_WRITE, value);
1839 }
1840
1841 /******************************************************************************
1842  *
1843  *  cpia2_set_saturation
1844  *
1845  *****************************************************************************/
1846 void cpia2_set_saturation(struct camera_data *cam, unsigned char value)
1847 {
1848         DBG("Setting saturation to %d (0x%0x)\n", value, value);
1849         cam->params.color_params.saturation = value;
1850         cpia2_do_command(cam,CPIA2_CMD_SET_VP_SATURATION, TRANSFER_WRITE,value);
1851 }
1852
1853 /******************************************************************************
1854  *
1855  *  wake_system
1856  *
1857  *****************************************************************************/
1858 void wake_system(struct camera_data *cam)
1859 {
1860         cpia2_do_command(cam, CPIA2_CMD_SET_WAKEUP, TRANSFER_WRITE, 0);
1861 }
1862
1863 /******************************************************************************
1864  *
1865  *  set_lowlight_boost
1866  *
1867  *  Valid for STV500 sensor only
1868  *****************************************************************************/
1869 void set_lowlight_boost(struct camera_data *cam)
1870 {
1871         struct cpia2_command cmd;
1872
1873         if (cam->params.pnp_id.device_type != DEVICE_STV_672 ||
1874             cam->params.version.sensor_flags != CPIA2_VP_SENSOR_FLAGS_500)
1875                 return;
1876
1877         cmd.direction = TRANSFER_WRITE;
1878         cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
1879         cmd.reg_count = 3;
1880         cmd.start = CPIA2_VP_RAM_ADDR_H;
1881
1882         cmd.buffer.block_data[0] = 0;   /* High byte of address to write to */
1883         cmd.buffer.block_data[1] = 0x59;        /* Low byte of address to write to */
1884         cmd.buffer.block_data[2] = 0;   /* High byte of data to write */
1885
1886         cpia2_send_command(cam, &cmd);
1887
1888         if (cam->params.vp_params.lowlight_boost) {
1889                 cmd.buffer.block_data[0] = 0x02;        /* Low byte data to write */
1890         } else {
1891                 cmd.buffer.block_data[0] = 0x06;
1892         }
1893         cmd.start = CPIA2_VP_RAM_DATA;
1894         cmd.reg_count = 1;
1895         cpia2_send_command(cam, &cmd);
1896
1897         /* Rehash the VP4 values */
1898         cpia2_do_command(cam, CPIA2_CMD_REHASH_VP4, TRANSFER_WRITE, 1);
1899 }
1900
1901 /******************************************************************************
1902  *
1903  *  cpia2_set_format
1904  *
1905  *  Assumes that new size is already set in param struct.
1906  *****************************************************************************/
1907 void cpia2_set_format(struct camera_data *cam)
1908 {
1909         cam->flush = true;
1910
1911         cpia2_usb_stream_pause(cam);
1912
1913         /* reset camera to new size */
1914         cpia2_set_low_power(cam);
1915         cpia2_reset_camera(cam);
1916         cam->flush = false;
1917
1918         cpia2_dbg_dump_registers(cam);
1919
1920         cpia2_usb_stream_resume(cam);
1921 }
1922
1923 /******************************************************************************
1924  *
1925  * cpia2_dbg_dump_registers
1926  *
1927  *****************************************************************************/
1928 void cpia2_dbg_dump_registers(struct camera_data *cam)
1929 {
1930 #ifdef _CPIA2_DEBUG_
1931         struct cpia2_command cmd;
1932
1933         if (!(debugs_on & DEBUG_DUMP_REGS))
1934                 return;
1935
1936         cmd.direction = TRANSFER_READ;
1937
1938         /* Start with bank 0 (SYSTEM) */
1939         cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_SYSTEM;
1940         cmd.reg_count = 3;
1941         cmd.start = 0;
1942         cpia2_send_command(cam, &cmd);
1943         printk(KERN_DEBUG "System Device Hi      = 0x%X\n",
1944                cmd.buffer.block_data[0]);
1945         printk(KERN_DEBUG "System Device Lo      = 0x%X\n",
1946                cmd.buffer.block_data[1]);
1947         printk(KERN_DEBUG "System_system control = 0x%X\n",
1948                cmd.buffer.block_data[2]);
1949
1950         /* Bank 1 (VC) */
1951         cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VC;
1952         cmd.reg_count = 4;
1953         cmd.start = 0x80;
1954         cpia2_send_command(cam, &cmd);
1955         printk(KERN_DEBUG "ASIC_ID       = 0x%X\n",
1956                cmd.buffer.block_data[0]);
1957         printk(KERN_DEBUG "ASIC_REV      = 0x%X\n",
1958                cmd.buffer.block_data[1]);
1959         printk(KERN_DEBUG "PW_CONTRL     = 0x%X\n",
1960                cmd.buffer.block_data[2]);
1961         printk(KERN_DEBUG "WAKEUP        = 0x%X\n",
1962                cmd.buffer.block_data[3]);
1963
1964         cmd.start = 0xA0;       /* ST_CTRL */
1965         cmd.reg_count = 1;
1966         cpia2_send_command(cam, &cmd);
1967         printk(KERN_DEBUG "Stream ctrl   = 0x%X\n",
1968                cmd.buffer.block_data[0]);
1969
1970         cmd.start = 0xA4;       /* Stream status */
1971         cpia2_send_command(cam, &cmd);
1972         printk(KERN_DEBUG "Stream status = 0x%X\n",
1973                cmd.buffer.block_data[0]);
1974
1975         cmd.start = 0xA8;       /* USB status */
1976         cmd.reg_count = 3;
1977         cpia2_send_command(cam, &cmd);
1978         printk(KERN_DEBUG "USB_CTRL      = 0x%X\n",
1979                cmd.buffer.block_data[0]);
1980         printk(KERN_DEBUG "USB_STRM      = 0x%X\n",
1981                cmd.buffer.block_data[1]);
1982         printk(KERN_DEBUG "USB_STATUS    = 0x%X\n",
1983                cmd.buffer.block_data[2]);
1984
1985         cmd.start = 0xAF;       /* USB settings */
1986         cmd.reg_count = 1;
1987         cpia2_send_command(cam, &cmd);
1988         printk(KERN_DEBUG "USB settings  = 0x%X\n",
1989                cmd.buffer.block_data[0]);
1990
1991         cmd.start = 0xC0;       /* VC stuff */
1992         cmd.reg_count = 26;
1993         cpia2_send_command(cam, &cmd);
1994         printk(KERN_DEBUG "VC Control    = 0x%0X\n",
1995                cmd.buffer.block_data[0]);
1996         printk(KERN_DEBUG "VC Format     = 0x%0X\n",
1997                cmd.buffer.block_data[3]);
1998         printk(KERN_DEBUG "VC Clocks     = 0x%0X\n",
1999                cmd.buffer.block_data[4]);
2000         printk(KERN_DEBUG "VC IHSize     = 0x%0X\n",
2001                cmd.buffer.block_data[5]);
2002         printk(KERN_DEBUG "VC Xlim Hi    = 0x%0X\n",
2003                cmd.buffer.block_data[6]);
2004         printk(KERN_DEBUG "VC XLim Lo    = 0x%0X\n",
2005                cmd.buffer.block_data[7]);
2006         printk(KERN_DEBUG "VC YLim Hi    = 0x%0X\n",
2007                cmd.buffer.block_data[8]);
2008         printk(KERN_DEBUG "VC YLim Lo    = 0x%0X\n",
2009                cmd.buffer.block_data[9]);
2010         printk(KERN_DEBUG "VC OHSize     = 0x%0X\n",
2011                cmd.buffer.block_data[10]);
2012         printk(KERN_DEBUG "VC OVSize     = 0x%0X\n",
2013                cmd.buffer.block_data[11]);
2014         printk(KERN_DEBUG "VC HCrop      = 0x%0X\n",
2015                cmd.buffer.block_data[12]);
2016         printk(KERN_DEBUG "VC VCrop      = 0x%0X\n",
2017                cmd.buffer.block_data[13]);
2018         printk(KERN_DEBUG "VC HPhase     = 0x%0X\n",
2019                cmd.buffer.block_data[14]);
2020         printk(KERN_DEBUG "VC VPhase     = 0x%0X\n",
2021                cmd.buffer.block_data[15]);
2022         printk(KERN_DEBUG "VC HIspan     = 0x%0X\n",
2023                cmd.buffer.block_data[16]);
2024         printk(KERN_DEBUG "VC VIspan     = 0x%0X\n",
2025                cmd.buffer.block_data[17]);
2026         printk(KERN_DEBUG "VC HiCrop     = 0x%0X\n",
2027                cmd.buffer.block_data[18]);
2028         printk(KERN_DEBUG "VC ViCrop     = 0x%0X\n",
2029                cmd.buffer.block_data[19]);
2030         printk(KERN_DEBUG "VC HiFract    = 0x%0X\n",
2031                cmd.buffer.block_data[20]);
2032         printk(KERN_DEBUG "VC ViFract    = 0x%0X\n",
2033                cmd.buffer.block_data[21]);
2034         printk(KERN_DEBUG "VC JPeg Opt   = 0x%0X\n",
2035                cmd.buffer.block_data[22]);
2036         printk(KERN_DEBUG "VC Creep Per  = 0x%0X\n",
2037                cmd.buffer.block_data[23]);
2038         printk(KERN_DEBUG "VC User Sq.   = 0x%0X\n",
2039                cmd.buffer.block_data[24]);
2040         printk(KERN_DEBUG "VC Target KB  = 0x%0X\n",
2041                cmd.buffer.block_data[25]);
2042
2043         /*** VP ***/
2044         cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VP;
2045         cmd.reg_count = 14;
2046         cmd.start = 0;
2047         cpia2_send_command(cam, &cmd);
2048
2049         printk(KERN_DEBUG "VP Dev Hi     = 0x%0X\n",
2050                cmd.buffer.block_data[0]);
2051         printk(KERN_DEBUG "VP Dev Lo     = 0x%0X\n",
2052                cmd.buffer.block_data[1]);
2053         printk(KERN_DEBUG "VP Sys State  = 0x%0X\n",
2054                cmd.buffer.block_data[2]);
2055         printk(KERN_DEBUG "VP Sys Ctrl   = 0x%0X\n",
2056                cmd.buffer.block_data[3]);
2057         printk(KERN_DEBUG "VP Sensor flg = 0x%0X\n",
2058                cmd.buffer.block_data[5]);
2059         printk(KERN_DEBUG "VP Sensor Rev = 0x%0X\n",
2060                cmd.buffer.block_data[6]);
2061         printk(KERN_DEBUG "VP Dev Config = 0x%0X\n",
2062                cmd.buffer.block_data[7]);
2063         printk(KERN_DEBUG "VP GPIO_DIR   = 0x%0X\n",
2064                cmd.buffer.block_data[8]);
2065         printk(KERN_DEBUG "VP GPIO_DATA  = 0x%0X\n",
2066                cmd.buffer.block_data[9]);
2067         printk(KERN_DEBUG "VP Ram ADDR H = 0x%0X\n",
2068                cmd.buffer.block_data[10]);
2069         printk(KERN_DEBUG "VP Ram ADDR L = 0x%0X\n",
2070                cmd.buffer.block_data[11]);
2071         printk(KERN_DEBUG "VP RAM Data   = 0x%0X\n",
2072                cmd.buffer.block_data[12]);
2073         printk(KERN_DEBUG "Do Call       = 0x%0X\n",
2074                cmd.buffer.block_data[13]);
2075
2076         if (cam->params.pnp_id.device_type == DEVICE_STV_672) {
2077                 cmd.reg_count = 9;
2078                 cmd.start = 0x0E;
2079                 cpia2_send_command(cam, &cmd);
2080                 printk(KERN_DEBUG "VP Clock Ctrl = 0x%0X\n",
2081                        cmd.buffer.block_data[0]);
2082                 printk(KERN_DEBUG "VP Patch Rev  = 0x%0X\n",
2083                        cmd.buffer.block_data[1]);
2084                 printk(KERN_DEBUG "VP Vid Mode   = 0x%0X\n",
2085                        cmd.buffer.block_data[2]);
2086                 printk(KERN_DEBUG "VP Framerate  = 0x%0X\n",
2087                        cmd.buffer.block_data[3]);
2088                 printk(KERN_DEBUG "VP UserEffect = 0x%0X\n",
2089                        cmd.buffer.block_data[4]);
2090                 printk(KERN_DEBUG "VP White Bal  = 0x%0X\n",
2091                        cmd.buffer.block_data[5]);
2092                 printk(KERN_DEBUG "VP WB thresh  = 0x%0X\n",
2093                        cmd.buffer.block_data[6]);
2094                 printk(KERN_DEBUG "VP Exp Modes  = 0x%0X\n",
2095                        cmd.buffer.block_data[7]);
2096                 printk(KERN_DEBUG "VP Exp Target = 0x%0X\n",
2097                        cmd.buffer.block_data[8]);
2098
2099                 cmd.reg_count = 1;
2100                 cmd.start = 0x1B;
2101                 cpia2_send_command(cam, &cmd);
2102                 printk(KERN_DEBUG "VP FlickerMds = 0x%0X\n",
2103                        cmd.buffer.block_data[0]);
2104         } else {
2105                 cmd.reg_count = 8 ;
2106                 cmd.start = 0x0E;
2107                 cpia2_send_command(cam, &cmd);
2108                 printk(KERN_DEBUG "VP Clock Ctrl = 0x%0X\n",
2109                        cmd.buffer.block_data[0]);
2110                 printk(KERN_DEBUG "VP Patch Rev  = 0x%0X\n",
2111                        cmd.buffer.block_data[1]);
2112                 printk(KERN_DEBUG "VP Vid Mode   = 0x%0X\n",
2113                        cmd.buffer.block_data[5]);
2114                 printk(KERN_DEBUG "VP Framerate  = 0x%0X\n",
2115                        cmd.buffer.block_data[6]);
2116                 printk(KERN_DEBUG "VP UserEffect = 0x%0X\n",
2117                        cmd.buffer.block_data[7]);
2118
2119                 cmd.reg_count = 1;
2120                 cmd.start = CPIA2_VP5_EXPOSURE_TARGET;
2121                 cpia2_send_command(cam, &cmd);
2122                 printk(KERN_DEBUG "VP5 Exp Target= 0x%0X\n",
2123                        cmd.buffer.block_data[0]);
2124
2125                 cmd.reg_count = 4;
2126                 cmd.start = 0x3A;
2127                 cpia2_send_command(cam, &cmd);
2128                 printk(KERN_DEBUG "VP5 MY Black  = 0x%0X\n",
2129                        cmd.buffer.block_data[0]);
2130                 printk(KERN_DEBUG "VP5 MCY Range = 0x%0X\n",
2131                        cmd.buffer.block_data[1]);
2132                 printk(KERN_DEBUG "VP5 MYCEILING = 0x%0X\n",
2133                        cmd.buffer.block_data[2]);
2134                 printk(KERN_DEBUG "VP5 MCUV Sat  = 0x%0X\n",
2135                        cmd.buffer.block_data[3]);
2136         }
2137 #endif
2138 }
2139
2140 /******************************************************************************
2141  *
2142  *  reset_camera_struct
2143  *
2144  *  Sets all values to the defaults
2145  *****************************************************************************/
2146 void reset_camera_struct(struct camera_data *cam)
2147 {
2148         /***
2149          * The following parameter values are the defaults from the register map.
2150          ***/
2151         cam->params.color_params.brightness = DEFAULT_BRIGHTNESS;
2152         cam->params.color_params.contrast = DEFAULT_CONTRAST;
2153         cam->params.color_params.saturation = DEFAULT_SATURATION;
2154         cam->params.vp_params.lowlight_boost = 0;
2155
2156         /* FlickerModes */
2157         cam->params.flicker_control.flicker_mode_req = NEVER_FLICKER;
2158         cam->params.flicker_control.mains_frequency = 60;
2159
2160         /* jpeg params */
2161         cam->params.compression.jpeg_options = CPIA2_VC_VC_JPEG_OPT_DEFAULT;
2162         cam->params.compression.creep_period = 2;
2163         cam->params.compression.user_squeeze = 20;
2164         cam->params.compression.inhibit_htables = false;
2165
2166         /* gpio params */
2167         cam->params.vp_params.gpio_direction = 0;       /* write, the default safe mode */
2168         cam->params.vp_params.gpio_data = 0;
2169
2170         /* Target kb params */
2171         cam->params.vc_params.target_kb = DEFAULT_TARGET_KB;
2172
2173         /***
2174          * Set Sensor FPS as fast as possible.
2175          ***/
2176         if(cam->params.pnp_id.device_type == DEVICE_STV_672) {
2177                 if(cam->params.version.sensor_flags == CPIA2_VP_SENSOR_FLAGS_500)
2178                         cam->params.vp_params.frame_rate = CPIA2_VP_FRAMERATE_15;
2179                 else
2180                         cam->params.vp_params.frame_rate = CPIA2_VP_FRAMERATE_30;
2181         } else {
2182                 cam->params.vp_params.frame_rate = CPIA2_VP_FRAMERATE_30;
2183         }
2184
2185         /***
2186          * Set default video mode as large as possible :
2187          * for vga sensor set to vga, for cif sensor set to CIF.
2188          ***/
2189         if (cam->params.version.sensor_flags == CPIA2_VP_SENSOR_FLAGS_500) {
2190                 cam->sensor_type = CPIA2_SENSOR_500;
2191                 cam->video_size = VIDEOSIZE_VGA;
2192                 cam->params.roi.width = STV_IMAGE_VGA_COLS;
2193                 cam->params.roi.height = STV_IMAGE_VGA_ROWS;
2194         } else {
2195                 cam->sensor_type = CPIA2_SENSOR_410;
2196                 cam->video_size = VIDEOSIZE_CIF;
2197                 cam->params.roi.width = STV_IMAGE_CIF_COLS;
2198                 cam->params.roi.height = STV_IMAGE_CIF_ROWS;
2199         }
2200
2201         /***
2202          * Fill in the v4l structures.  video_cap is filled in inside the VIDIOCCAP
2203          * Ioctl.  Here, just do the window and picture stucts.
2204          ***/
2205         cam->vp.palette = (u16) VIDEO_PALETTE_RGB24;    /* Is this right? */
2206         cam->vp.brightness = (u16) cam->params.color_params.brightness * 256;
2207         cam->vp.colour = (u16) cam->params.color_params.saturation * 256;
2208         cam->vp.contrast = (u16) cam->params.color_params.contrast * 256;
2209
2210         cam->vw.x = 0;
2211         cam->vw.y = 0;
2212         cam->vw.width = cam->params.roi.width;
2213         cam->vw.height = cam->params.roi.height;
2214         cam->vw.flags = 0;
2215         cam->vw.clipcount = 0;
2216
2217         return;
2218 }
2219
2220 /******************************************************************************
2221  *
2222  *  cpia2_init_camera_struct
2223  *
2224  *  Initializes camera struct, does not call reset to fill in defaults.
2225  *****************************************************************************/
2226 struct camera_data *cpia2_init_camera_struct(void)
2227 {
2228         struct camera_data *cam;
2229
2230         cam = kmalloc(sizeof(*cam), GFP_KERNEL);
2231
2232         if (!cam) {
2233                 ERR("couldn't kmalloc cpia2 struct\n");
2234                 return NULL;
2235         }
2236
2237         /* Default everything to 0 */
2238         memset(cam, 0, sizeof(struct camera_data));
2239
2240         cam->present = 1;
2241         mutex_init(&cam->busy_lock);
2242         init_waitqueue_head(&cam->wq_stream);
2243
2244         return cam;
2245 }
2246
2247 /******************************************************************************
2248  *
2249  *  cpia2_init_camera
2250  *
2251  *  Initializes camera.
2252  *****************************************************************************/
2253 int cpia2_init_camera(struct camera_data *cam)
2254 {
2255         DBG("Start\n");
2256
2257         cam->mmapped = false;
2258
2259         /* Get sensor and asic types before reset. */
2260         cpia2_set_high_power(cam);
2261         cpia2_get_version_info(cam);
2262         if (cam->params.version.asic_id != CPIA2_ASIC_672) {
2263                 ERR("Device IO error (asicID has incorrect value of 0x%X\n",
2264                     cam->params.version.asic_id);
2265                 return -ENODEV;
2266         }
2267
2268         /* Set GPIO direction and data to a safe state. */
2269         cpia2_do_command(cam, CPIA2_CMD_SET_VC_MP_GPIO_DIRECTION,
2270                          TRANSFER_WRITE, 0);
2271         cpia2_do_command(cam, CPIA2_CMD_SET_VC_MP_GPIO_DATA,
2272                          TRANSFER_WRITE, 0);
2273
2274         /* resetting struct requires version info for sensor and asic types */
2275         reset_camera_struct(cam);
2276
2277         cpia2_set_low_power(cam);
2278
2279         DBG("End\n");
2280
2281         return 0;
2282 }
2283
2284 /******************************************************************************
2285  *
2286  *  cpia2_allocate_buffers
2287  *
2288  *****************************************************************************/
2289 int cpia2_allocate_buffers(struct camera_data *cam)
2290 {
2291         int i;
2292
2293         if(!cam->buffers) {
2294                 u32 size = cam->num_frames*sizeof(struct framebuf);
2295                 cam->buffers = kmalloc(size, GFP_KERNEL);
2296                 if(!cam->buffers) {
2297                         ERR("couldn't kmalloc frame buffer structures\n");
2298                         return -ENOMEM;
2299                 }
2300         }
2301
2302         if(!cam->frame_buffer) {
2303                 cam->frame_buffer = rvmalloc(cam->frame_size*cam->num_frames);
2304                 if (!cam->frame_buffer) {
2305                         ERR("couldn't vmalloc frame buffer data area\n");
2306                         kfree(cam->buffers);
2307                         cam->buffers = NULL;
2308                         return -ENOMEM;
2309                 }
2310         }
2311
2312         for(i=0; i<cam->num_frames-1; ++i) {
2313                 cam->buffers[i].next = &cam->buffers[i+1];
2314                 cam->buffers[i].data = cam->frame_buffer +i*cam->frame_size;
2315                 cam->buffers[i].status = FRAME_EMPTY;
2316                 cam->buffers[i].length = 0;
2317                 cam->buffers[i].max_length = 0;
2318                 cam->buffers[i].num = i;
2319         }
2320         cam->buffers[i].next = cam->buffers;
2321         cam->buffers[i].data = cam->frame_buffer +i*cam->frame_size;
2322         cam->buffers[i].status = FRAME_EMPTY;
2323         cam->buffers[i].length = 0;
2324         cam->buffers[i].max_length = 0;
2325         cam->buffers[i].num = i;
2326         cam->curbuff = cam->buffers;
2327         cam->workbuff = cam->curbuff->next;
2328         DBG("buffers=%p, curbuff=%p, workbuff=%p\n", cam->buffers, cam->curbuff,
2329             cam->workbuff);
2330         return 0;
2331 }
2332
2333 /******************************************************************************
2334  *
2335  *  cpia2_free_buffers
2336  *
2337  *****************************************************************************/
2338 void cpia2_free_buffers(struct camera_data *cam)
2339 {
2340         if(cam->buffers) {
2341                 kfree(cam->buffers);
2342                 cam->buffers = NULL;
2343         }
2344         if(cam->frame_buffer) {
2345                 rvfree(cam->frame_buffer, cam->frame_size*cam->num_frames);
2346                 cam->frame_buffer = NULL;
2347         }
2348 }
2349
2350 /******************************************************************************
2351  *
2352  *  cpia2_read
2353  *
2354  *****************************************************************************/
2355 long cpia2_read(struct camera_data *cam,
2356                 char __user *buf, unsigned long count, int noblock)
2357 {
2358         struct framebuf *frame;
2359         if (!count) {
2360                 return 0;
2361         }
2362
2363         if (!buf) {
2364                 ERR("%s: buffer NULL\n",__FUNCTION__);
2365                 return -EINVAL;
2366         }
2367
2368         if (!cam) {
2369                 ERR("%s: Internal error, camera_data NULL!\n",__FUNCTION__);
2370                 return -EINVAL;
2371         }
2372
2373         /* make this _really_ smp and multithread-safe */
2374         if (mutex_lock_interruptible(&cam->busy_lock))
2375                 return -ERESTARTSYS;
2376
2377         if (!cam->present) {
2378                 LOG("%s: camera removed\n",__FUNCTION__);
2379                 mutex_unlock(&cam->busy_lock);
2380                 return 0;       /* EOF */
2381         }
2382
2383         if(!cam->streaming) {
2384                 /* Start streaming */
2385                 cpia2_usb_stream_start(cam,
2386                                        cam->params.camera_state.stream_mode);
2387         }
2388
2389         /* Copy cam->curbuff in case it changes while we're processing */
2390         frame = cam->curbuff;
2391         if (noblock && frame->status != FRAME_READY) {
2392                 mutex_unlock(&cam->busy_lock);
2393                 return -EAGAIN;
2394         }
2395
2396         if(frame->status != FRAME_READY) {
2397                 mutex_unlock(&cam->busy_lock);
2398                 wait_event_interruptible(cam->wq_stream,
2399                                !cam->present ||
2400                                (frame = cam->curbuff)->status == FRAME_READY);
2401                 if (signal_pending(current))
2402                         return -ERESTARTSYS;
2403                 /* make this _really_ smp and multithread-safe */
2404                 if (mutex_lock_interruptible(&cam->busy_lock)) {
2405                         return -ERESTARTSYS;
2406                 }
2407                 if(!cam->present) {
2408                         mutex_unlock(&cam->busy_lock);
2409                         return 0;
2410                 }
2411         }
2412
2413         /* copy data to user space */
2414         if (frame->length > count) {
2415                 mutex_unlock(&cam->busy_lock);
2416                 return -EFAULT;
2417         }
2418         if (copy_to_user(buf, frame->data, frame->length)) {
2419                 mutex_unlock(&cam->busy_lock);
2420                 return -EFAULT;
2421         }
2422
2423         count = frame->length;
2424
2425         frame->status = FRAME_EMPTY;
2426
2427         mutex_unlock(&cam->busy_lock);
2428         return count;
2429 }
2430
2431 /******************************************************************************
2432  *
2433  *  cpia2_poll
2434  *
2435  *****************************************************************************/
2436 unsigned int cpia2_poll(struct camera_data *cam, struct file *filp,
2437                         poll_table *wait)
2438 {
2439         unsigned int status=0;
2440
2441         if(!cam) {
2442                 ERR("%s: Internal error, camera_data not found!\n",__FUNCTION__);
2443                 return POLLERR;
2444         }
2445
2446         mutex_lock(&cam->busy_lock);
2447
2448         if(!cam->present) {
2449                 mutex_unlock(&cam->busy_lock);
2450                 return POLLHUP;
2451         }
2452
2453         if(!cam->streaming) {
2454                 /* Start streaming */
2455                 cpia2_usb_stream_start(cam,
2456                                        cam->params.camera_state.stream_mode);
2457         }
2458
2459         mutex_unlock(&cam->busy_lock);
2460         poll_wait(filp, &cam->wq_stream, wait);
2461         mutex_lock(&cam->busy_lock);
2462
2463         if(!cam->present)
2464                 status = POLLHUP;
2465         else if(cam->curbuff->status == FRAME_READY)
2466                 status = POLLIN | POLLRDNORM;
2467
2468         mutex_unlock(&cam->busy_lock);
2469         return status;
2470 }
2471
2472 /******************************************************************************
2473  *
2474  *  cpia2_remap_buffer
2475  *
2476  *****************************************************************************/
2477 int cpia2_remap_buffer(struct camera_data *cam, struct vm_area_struct *vma)
2478 {
2479         const char *adr = (const char *)vma->vm_start;
2480         unsigned long size = vma->vm_end-vma->vm_start;
2481         unsigned long start_offset = vma->vm_pgoff << PAGE_SHIFT;
2482         unsigned long start = (unsigned long) adr;
2483         unsigned long page, pos;
2484
2485         if (!cam)
2486                 return -ENODEV;
2487
2488         DBG("mmap offset:%ld size:%ld\n", start_offset, size);
2489
2490         /* make this _really_ smp-safe */
2491         if (mutex_lock_interruptible(&cam->busy_lock))
2492                 return -ERESTARTSYS;
2493
2494         if (!cam->present) {
2495                 mutex_unlock(&cam->busy_lock);
2496                 return -ENODEV;
2497         }
2498
2499         if (size > cam->frame_size*cam->num_frames  ||
2500             (start_offset % cam->frame_size) != 0 ||
2501             (start_offset+size > cam->frame_size*cam->num_frames)) {
2502                 mutex_unlock(&cam->busy_lock);
2503                 return -EINVAL;
2504         }
2505
2506         pos = ((unsigned long) (cam->frame_buffer)) + start_offset;
2507         while (size > 0) {
2508                 page = kvirt_to_pa(pos);
2509                 if (remap_pfn_range(vma, start, page >> PAGE_SHIFT, PAGE_SIZE, PAGE_SHARED)) {
2510                         mutex_unlock(&cam->busy_lock);
2511                         return -EAGAIN;
2512                 }
2513                 start += PAGE_SIZE;
2514                 pos += PAGE_SIZE;
2515                 if (size > PAGE_SIZE)
2516                         size -= PAGE_SIZE;
2517                 else
2518                         size = 0;
2519         }
2520
2521         cam->mmapped = true;
2522         mutex_unlock(&cam->busy_lock);
2523         return 0;
2524 }
2525