cpia2_usb.c 26 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899900901902903904905906907908909910911912913914915916917918919920921922923924925926927928929930931932933934935936937938939940941942943944945946947948949950951952953954955956957
  1. /****************************************************************************
  2. *
  3. * Filename: cpia2_usb.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@lxorguk.ukuu.org.uk>
  29. ****************************************************************************/
  30. #include <linux/kernel.h>
  31. #include <linux/slab.h>
  32. #include <linux/usb.h>
  33. #include <linux/module.h>
  34. #include "cpia2.h"
  35. static int frame_sizes[] = {
  36. 0, // USBIF_CMDONLY
  37. 0, // USBIF_BULK
  38. 128, // USBIF_ISO_1
  39. 384, // USBIF_ISO_2
  40. 640, // USBIF_ISO_3
  41. 768, // USBIF_ISO_4
  42. 896, // USBIF_ISO_5
  43. 1023, // USBIF_ISO_6
  44. };
  45. #define FRAMES_PER_DESC 10
  46. #define FRAME_SIZE_PER_DESC frame_sizes[cam->cur_alt]
  47. static void process_frame(struct camera_data *cam);
  48. static void cpia2_usb_complete(struct urb *urb);
  49. static int cpia2_usb_probe(struct usb_interface *intf,
  50. const struct usb_device_id *id);
  51. static void cpia2_usb_disconnect(struct usb_interface *intf);
  52. static int cpia2_usb_suspend(struct usb_interface *intf, pm_message_t message);
  53. static int cpia2_usb_resume(struct usb_interface *intf);
  54. static void free_sbufs(struct camera_data *cam);
  55. static void add_APPn(struct camera_data *cam);
  56. static void add_COM(struct camera_data *cam);
  57. static int submit_urbs(struct camera_data *cam);
  58. static int set_alternate(struct camera_data *cam, unsigned int alt);
  59. static int configure_transfer_mode(struct camera_data *cam, unsigned int alt);
  60. static struct usb_device_id cpia2_id_table[] = {
  61. {USB_DEVICE(0x0553, 0x0100)},
  62. {USB_DEVICE(0x0553, 0x0140)},
  63. {USB_DEVICE(0x0553, 0x0151)}, /* STV0676 */
  64. {} /* Terminating entry */
  65. };
  66. MODULE_DEVICE_TABLE(usb, cpia2_id_table);
  67. static struct usb_driver cpia2_driver = {
  68. .name = "cpia2",
  69. .probe = cpia2_usb_probe,
  70. .disconnect = cpia2_usb_disconnect,
  71. .suspend = cpia2_usb_suspend,
  72. .resume = cpia2_usb_resume,
  73. .reset_resume = cpia2_usb_resume,
  74. .id_table = cpia2_id_table
  75. };
  76. /******************************************************************************
  77. *
  78. * process_frame
  79. *
  80. *****************************************************************************/
  81. static void process_frame(struct camera_data *cam)
  82. {
  83. static int frame_count;
  84. unsigned char *inbuff = cam->workbuff->data;
  85. DBG("Processing frame #%d, current:%d\n",
  86. cam->workbuff->num, cam->curbuff->num);
  87. if(cam->workbuff->length > cam->workbuff->max_length)
  88. cam->workbuff->max_length = cam->workbuff->length;
  89. if ((inbuff[0] == 0xFF) && (inbuff[1] == 0xD8)) {
  90. frame_count++;
  91. } else {
  92. cam->workbuff->status = FRAME_ERROR;
  93. DBG("Start of frame not found\n");
  94. return;
  95. }
  96. /***
  97. * Now the output buffer should have a JPEG image in it.
  98. ***/
  99. if(!cam->first_image_seen) {
  100. /* Always skip the first image after streaming
  101. * starts. It is almost certainly corrupt. */
  102. cam->first_image_seen = 1;
  103. cam->workbuff->status = FRAME_EMPTY;
  104. return;
  105. }
  106. if (cam->workbuff->length > 3) {
  107. if(cam->mmapped &&
  108. cam->workbuff->length < cam->workbuff->max_length) {
  109. /* No junk in the buffers */
  110. memset(cam->workbuff->data+cam->workbuff->length,
  111. 0, cam->workbuff->max_length-
  112. cam->workbuff->length);
  113. }
  114. cam->workbuff->max_length = cam->workbuff->length;
  115. cam->workbuff->status = FRAME_READY;
  116. if(!cam->mmapped && cam->num_frames > 2) {
  117. /* During normal reading, the most recent
  118. * frame will be read. If the current frame
  119. * hasn't started reading yet, it will never
  120. * be read, so mark it empty. If the buffer is
  121. * mmapped, or we have few buffers, we need to
  122. * wait for the user to free the buffer.
  123. *
  124. * NOTE: This is not entirely foolproof with 3
  125. * buffers, but it would take an EXTREMELY
  126. * overloaded system to cause problems (possible
  127. * image data corruption). Basically, it would
  128. * need to take more time to execute cpia2_read
  129. * than it would for the camera to send
  130. * cam->num_frames-2 frames before problems
  131. * could occur.
  132. */
  133. cam->curbuff->status = FRAME_EMPTY;
  134. }
  135. cam->curbuff = cam->workbuff;
  136. cam->workbuff = cam->workbuff->next;
  137. DBG("Changed buffers, work:%d, current:%d\n",
  138. cam->workbuff->num, cam->curbuff->num);
  139. return;
  140. } else {
  141. DBG("Not enough data for an image.\n");
  142. }
  143. cam->workbuff->status = FRAME_ERROR;
  144. return;
  145. }
  146. /******************************************************************************
  147. *
  148. * add_APPn
  149. *
  150. * Adds a user specified APPn record
  151. *****************************************************************************/
  152. static void add_APPn(struct camera_data *cam)
  153. {
  154. if(cam->APP_len > 0) {
  155. cam->workbuff->data[cam->workbuff->length++] = 0xFF;
  156. cam->workbuff->data[cam->workbuff->length++] = 0xE0+cam->APPn;
  157. cam->workbuff->data[cam->workbuff->length++] = 0;
  158. cam->workbuff->data[cam->workbuff->length++] = cam->APP_len+2;
  159. memcpy(cam->workbuff->data+cam->workbuff->length,
  160. cam->APP_data, cam->APP_len);
  161. cam->workbuff->length += cam->APP_len;
  162. }
  163. }
  164. /******************************************************************************
  165. *
  166. * add_COM
  167. *
  168. * Adds a user specified COM record
  169. *****************************************************************************/
  170. static void add_COM(struct camera_data *cam)
  171. {
  172. if(cam->COM_len > 0) {
  173. cam->workbuff->data[cam->workbuff->length++] = 0xFF;
  174. cam->workbuff->data[cam->workbuff->length++] = 0xFE;
  175. cam->workbuff->data[cam->workbuff->length++] = 0;
  176. cam->workbuff->data[cam->workbuff->length++] = cam->COM_len+2;
  177. memcpy(cam->workbuff->data+cam->workbuff->length,
  178. cam->COM_data, cam->COM_len);
  179. cam->workbuff->length += cam->COM_len;
  180. }
  181. }
  182. /******************************************************************************
  183. *
  184. * cpia2_usb_complete
  185. *
  186. * callback when incoming packet is received
  187. *****************************************************************************/
  188. static void cpia2_usb_complete(struct urb *urb)
  189. {
  190. int i;
  191. unsigned char *cdata;
  192. static int frame_ready = false;
  193. struct camera_data *cam = (struct camera_data *) urb->context;
  194. if (urb->status!=0) {
  195. if (!(urb->status == -ENOENT ||
  196. urb->status == -ECONNRESET ||
  197. urb->status == -ESHUTDOWN))
  198. {
  199. DBG("urb->status = %d!\n", urb->status);
  200. }
  201. DBG("Stopping streaming\n");
  202. return;
  203. }
  204. if (!cam->streaming || !video_is_registered(&cam->vdev)) {
  205. LOG("Will now stop the streaming: streaming = %d, present=%d\n",
  206. cam->streaming, video_is_registered(&cam->vdev));
  207. return;
  208. }
  209. /***
  210. * Packet collater
  211. ***/
  212. //DBG("Collating %d packets\n", urb->number_of_packets);
  213. for (i = 0; i < urb->number_of_packets; i++) {
  214. u16 checksum, iso_checksum;
  215. int j;
  216. int n = urb->iso_frame_desc[i].actual_length;
  217. int st = urb->iso_frame_desc[i].status;
  218. if(cam->workbuff->status == FRAME_READY) {
  219. struct framebuf *ptr;
  220. /* Try to find an available buffer */
  221. DBG("workbuff full, searching\n");
  222. for (ptr = cam->workbuff->next;
  223. ptr != cam->workbuff;
  224. ptr = ptr->next)
  225. {
  226. if (ptr->status == FRAME_EMPTY) {
  227. ptr->status = FRAME_READING;
  228. ptr->length = 0;
  229. break;
  230. }
  231. }
  232. if (ptr == cam->workbuff)
  233. break; /* No READING or EMPTY buffers left */
  234. cam->workbuff = ptr;
  235. }
  236. if (cam->workbuff->status == FRAME_EMPTY ||
  237. cam->workbuff->status == FRAME_ERROR) {
  238. cam->workbuff->status = FRAME_READING;
  239. cam->workbuff->length = 0;
  240. }
  241. //DBG(" Packet %d length = %d, status = %d\n", i, n, st);
  242. cdata = urb->transfer_buffer + urb->iso_frame_desc[i].offset;
  243. if (st) {
  244. LOG("cpia2 data error: [%d] len=%d, status = %d\n",
  245. i, n, st);
  246. if(!ALLOW_CORRUPT)
  247. cam->workbuff->status = FRAME_ERROR;
  248. continue;
  249. }
  250. if(n<=2)
  251. continue;
  252. checksum = 0;
  253. for(j=0; j<n-2; ++j)
  254. checksum += cdata[j];
  255. iso_checksum = cdata[j] + cdata[j+1]*256;
  256. if(checksum != iso_checksum) {
  257. LOG("checksum mismatch: [%d] len=%d, calculated = %x, checksum = %x\n",
  258. i, n, (int)checksum, (int)iso_checksum);
  259. if(!ALLOW_CORRUPT) {
  260. cam->workbuff->status = FRAME_ERROR;
  261. continue;
  262. }
  263. }
  264. n -= 2;
  265. if(cam->workbuff->status != FRAME_READING) {
  266. if((0xFF == cdata[0] && 0xD8 == cdata[1]) ||
  267. (0xD8 == cdata[0] && 0xFF == cdata[1] &&
  268. 0 != cdata[2])) {
  269. /* frame is skipped, but increment total
  270. * frame count anyway */
  271. cam->frame_count++;
  272. }
  273. DBG("workbuff not reading, status=%d\n",
  274. cam->workbuff->status);
  275. continue;
  276. }
  277. if (cam->frame_size < cam->workbuff->length + n) {
  278. ERR("buffer overflow! length: %d, n: %d\n",
  279. cam->workbuff->length, n);
  280. cam->workbuff->status = FRAME_ERROR;
  281. if(cam->workbuff->length > cam->workbuff->max_length)
  282. cam->workbuff->max_length =
  283. cam->workbuff->length;
  284. continue;
  285. }
  286. if (cam->workbuff->length == 0) {
  287. int data_offset;
  288. if ((0xD8 == cdata[0]) && (0xFF == cdata[1])) {
  289. data_offset = 1;
  290. } else if((0xFF == cdata[0]) && (0xD8 == cdata[1])
  291. && (0xFF == cdata[2])) {
  292. data_offset = 2;
  293. } else {
  294. DBG("Ignoring packet, not beginning!\n");
  295. continue;
  296. }
  297. DBG("Start of frame pattern found\n");
  298. do_gettimeofday(&cam->workbuff->timestamp);
  299. cam->workbuff->seq = cam->frame_count++;
  300. cam->workbuff->data[0] = 0xFF;
  301. cam->workbuff->data[1] = 0xD8;
  302. cam->workbuff->length = 2;
  303. add_APPn(cam);
  304. add_COM(cam);
  305. memcpy(cam->workbuff->data+cam->workbuff->length,
  306. cdata+data_offset, n-data_offset);
  307. cam->workbuff->length += n-data_offset;
  308. } else if (cam->workbuff->length > 0) {
  309. memcpy(cam->workbuff->data + cam->workbuff->length,
  310. cdata, n);
  311. cam->workbuff->length += n;
  312. }
  313. if ((cam->workbuff->length >= 3) &&
  314. (cam->workbuff->data[cam->workbuff->length - 3] == 0xFF) &&
  315. (cam->workbuff->data[cam->workbuff->length - 2] == 0xD9) &&
  316. (cam->workbuff->data[cam->workbuff->length - 1] == 0xFF)) {
  317. frame_ready = true;
  318. cam->workbuff->data[cam->workbuff->length - 1] = 0;
  319. cam->workbuff->length -= 1;
  320. } else if ((cam->workbuff->length >= 2) &&
  321. (cam->workbuff->data[cam->workbuff->length - 2] == 0xFF) &&
  322. (cam->workbuff->data[cam->workbuff->length - 1] == 0xD9)) {
  323. frame_ready = true;
  324. }
  325. if (frame_ready) {
  326. DBG("Workbuff image size = %d\n",cam->workbuff->length);
  327. process_frame(cam);
  328. frame_ready = false;
  329. if (waitqueue_active(&cam->wq_stream))
  330. wake_up_interruptible(&cam->wq_stream);
  331. }
  332. }
  333. if(cam->streaming) {
  334. /* resubmit */
  335. urb->dev = cam->dev;
  336. if ((i = usb_submit_urb(urb, GFP_ATOMIC)) != 0)
  337. ERR("%s: usb_submit_urb ret %d!\n", __func__, i);
  338. }
  339. }
  340. /******************************************************************************
  341. *
  342. * configure_transfer_mode
  343. *
  344. *****************************************************************************/
  345. static int configure_transfer_mode(struct camera_data *cam, unsigned int alt)
  346. {
  347. static unsigned char iso_regs[8][4] = {
  348. {0x00, 0x00, 0x00, 0x00},
  349. {0x00, 0x00, 0x00, 0x00},
  350. {0xB9, 0x00, 0x00, 0x7E},
  351. {0xB9, 0x00, 0x01, 0x7E},
  352. {0xB9, 0x00, 0x02, 0x7E},
  353. {0xB9, 0x00, 0x02, 0xFE},
  354. {0xB9, 0x00, 0x03, 0x7E},
  355. {0xB9, 0x00, 0x03, 0xFD}
  356. };
  357. struct cpia2_command cmd;
  358. unsigned char reg;
  359. if (!video_is_registered(&cam->vdev))
  360. return -ENODEV;
  361. /***
  362. * Write the isoc registers according to the alternate selected
  363. ***/
  364. cmd.direction = TRANSFER_WRITE;
  365. cmd.buffer.block_data[0] = iso_regs[alt][0];
  366. cmd.buffer.block_data[1] = iso_regs[alt][1];
  367. cmd.buffer.block_data[2] = iso_regs[alt][2];
  368. cmd.buffer.block_data[3] = iso_regs[alt][3];
  369. cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VC;
  370. cmd.start = CPIA2_VC_USB_ISOLIM;
  371. cmd.reg_count = 4;
  372. cpia2_send_command(cam, &cmd);
  373. /***
  374. * Enable relevant streams before starting polling.
  375. * First read USB Stream Config Register.
  376. ***/
  377. cmd.direction = TRANSFER_READ;
  378. cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VC;
  379. cmd.start = CPIA2_VC_USB_STRM;
  380. cmd.reg_count = 1;
  381. cpia2_send_command(cam, &cmd);
  382. reg = cmd.buffer.block_data[0];
  383. /* Clear iso, bulk, and int */
  384. reg &= ~(CPIA2_VC_USB_STRM_BLK_ENABLE |
  385. CPIA2_VC_USB_STRM_ISO_ENABLE |
  386. CPIA2_VC_USB_STRM_INT_ENABLE);
  387. if (alt == USBIF_BULK) {
  388. DBG("Enabling bulk xfer\n");
  389. reg |= CPIA2_VC_USB_STRM_BLK_ENABLE; /* Enable Bulk */
  390. cam->xfer_mode = XFER_BULK;
  391. } else if (alt >= USBIF_ISO_1) {
  392. DBG("Enabling ISOC xfer\n");
  393. reg |= CPIA2_VC_USB_STRM_ISO_ENABLE;
  394. cam->xfer_mode = XFER_ISOC;
  395. }
  396. cmd.buffer.block_data[0] = reg;
  397. cmd.direction = TRANSFER_WRITE;
  398. cmd.start = CPIA2_VC_USB_STRM;
  399. cmd.reg_count = 1;
  400. cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VC;
  401. cpia2_send_command(cam, &cmd);
  402. return 0;
  403. }
  404. /******************************************************************************
  405. *
  406. * cpia2_usb_change_streaming_alternate
  407. *
  408. *****************************************************************************/
  409. int cpia2_usb_change_streaming_alternate(struct camera_data *cam,
  410. unsigned int alt)
  411. {
  412. int ret = 0;
  413. if(alt < USBIF_ISO_1 || alt > USBIF_ISO_6)
  414. return -EINVAL;
  415. if(alt == cam->params.camera_state.stream_mode)
  416. return 0;
  417. cpia2_usb_stream_pause(cam);
  418. configure_transfer_mode(cam, alt);
  419. cam->params.camera_state.stream_mode = alt;
  420. /* Reset the camera to prevent image quality degradation */
  421. cpia2_reset_camera(cam);
  422. cpia2_usb_stream_resume(cam);
  423. return ret;
  424. }
  425. /******************************************************************************
  426. *
  427. * set_alternate
  428. *
  429. *****************************************************************************/
  430. static int set_alternate(struct camera_data *cam, unsigned int alt)
  431. {
  432. int ret = 0;
  433. if(alt == cam->cur_alt)
  434. return 0;
  435. if (cam->cur_alt != USBIF_CMDONLY) {
  436. DBG("Changing from alt %d to %d\n", cam->cur_alt, USBIF_CMDONLY);
  437. ret = usb_set_interface(cam->dev, cam->iface, USBIF_CMDONLY);
  438. if (ret != 0)
  439. return ret;
  440. }
  441. if (alt != USBIF_CMDONLY) {
  442. DBG("Changing from alt %d to %d\n", USBIF_CMDONLY, alt);
  443. ret = usb_set_interface(cam->dev, cam->iface, alt);
  444. if (ret != 0)
  445. return ret;
  446. }
  447. cam->old_alt = cam->cur_alt;
  448. cam->cur_alt = alt;
  449. return ret;
  450. }
  451. /******************************************************************************
  452. *
  453. * free_sbufs
  454. *
  455. * Free all cam->sbuf[]. All non-NULL .data and .urb members that are non-NULL
  456. * are assumed to be allocated. Non-NULL .urb members are also assumed to be
  457. * submitted (and must therefore be killed before they are freed).
  458. *****************************************************************************/
  459. static void free_sbufs(struct camera_data *cam)
  460. {
  461. int i;
  462. for (i = 0; i < NUM_SBUF; i++) {
  463. if(cam->sbuf[i].urb) {
  464. usb_kill_urb(cam->sbuf[i].urb);
  465. usb_free_urb(cam->sbuf[i].urb);
  466. cam->sbuf[i].urb = NULL;
  467. }
  468. if(cam->sbuf[i].data) {
  469. kfree(cam->sbuf[i].data);
  470. cam->sbuf[i].data = NULL;
  471. }
  472. }
  473. }
  474. /*******
  475. * Convenience functions
  476. *******/
  477. /****************************************************************************
  478. *
  479. * write_packet
  480. *
  481. ***************************************************************************/
  482. static int write_packet(struct usb_device *udev,
  483. u8 request, u8 * registers, u16 start, size_t size)
  484. {
  485. if (!registers || size <= 0)
  486. return -EINVAL;
  487. return usb_control_msg(udev,
  488. usb_sndctrlpipe(udev, 0),
  489. request,
  490. USB_TYPE_VENDOR | USB_RECIP_DEVICE,
  491. start, /* value */
  492. 0, /* index */
  493. registers, /* buffer */
  494. size,
  495. HZ);
  496. }
  497. /****************************************************************************
  498. *
  499. * read_packet
  500. *
  501. ***************************************************************************/
  502. static int read_packet(struct usb_device *udev,
  503. u8 request, u8 * registers, u16 start, size_t size)
  504. {
  505. if (!registers || size <= 0)
  506. return -EINVAL;
  507. return usb_control_msg(udev,
  508. usb_rcvctrlpipe(udev, 0),
  509. request,
  510. USB_DIR_IN|USB_TYPE_VENDOR|USB_RECIP_DEVICE,
  511. start, /* value */
  512. 0, /* index */
  513. registers, /* buffer */
  514. size,
  515. HZ);
  516. }
  517. /******************************************************************************
  518. *
  519. * cpia2_usb_transfer_cmd
  520. *
  521. *****************************************************************************/
  522. int cpia2_usb_transfer_cmd(struct camera_data *cam,
  523. void *registers,
  524. u8 request, u8 start, u8 count, u8 direction)
  525. {
  526. int err = 0;
  527. struct usb_device *udev = cam->dev;
  528. if (!udev) {
  529. ERR("%s: Internal driver error: udev is NULL\n", __func__);
  530. return -EINVAL;
  531. }
  532. if (!registers) {
  533. ERR("%s: Internal driver error: register array is NULL\n", __func__);
  534. return -EINVAL;
  535. }
  536. if (direction == TRANSFER_READ) {
  537. err = read_packet(udev, request, (u8 *)registers, start, count);
  538. if (err > 0)
  539. err = 0;
  540. } else if (direction == TRANSFER_WRITE) {
  541. err =write_packet(udev, request, (u8 *)registers, start, count);
  542. if (err < 0) {
  543. LOG("Control message failed, err val = %d\n", err);
  544. LOG("Message: request = 0x%0X, start = 0x%0X\n",
  545. request, start);
  546. LOG("Message: count = %d, register[0] = 0x%0X\n",
  547. count, ((unsigned char *) registers)[0]);
  548. } else
  549. err=0;
  550. } else {
  551. LOG("Unexpected first byte of direction: %d\n",
  552. direction);
  553. return -EINVAL;
  554. }
  555. if(err != 0)
  556. LOG("Unexpected error: %d\n", err);
  557. return err;
  558. }
  559. /******************************************************************************
  560. *
  561. * submit_urbs
  562. *
  563. *****************************************************************************/
  564. static int submit_urbs(struct camera_data *cam)
  565. {
  566. struct urb *urb;
  567. int fx, err, i, j;
  568. for(i=0; i<NUM_SBUF; ++i) {
  569. if (cam->sbuf[i].data)
  570. continue;
  571. cam->sbuf[i].data =
  572. kmalloc(FRAMES_PER_DESC * FRAME_SIZE_PER_DESC, GFP_KERNEL);
  573. if (!cam->sbuf[i].data) {
  574. while (--i >= 0) {
  575. kfree(cam->sbuf[i].data);
  576. cam->sbuf[i].data = NULL;
  577. }
  578. return -ENOMEM;
  579. }
  580. }
  581. /* We double buffer the Isoc lists, and also know the polling
  582. * interval is every frame (1 == (1 << (bInterval -1))).
  583. */
  584. for(i=0; i<NUM_SBUF; ++i) {
  585. if(cam->sbuf[i].urb) {
  586. continue;
  587. }
  588. urb = usb_alloc_urb(FRAMES_PER_DESC, GFP_KERNEL);
  589. if (!urb) {
  590. ERR("%s: usb_alloc_urb error!\n", __func__);
  591. for (j = 0; j < i; j++)
  592. usb_free_urb(cam->sbuf[j].urb);
  593. return -ENOMEM;
  594. }
  595. cam->sbuf[i].urb = urb;
  596. urb->dev = cam->dev;
  597. urb->context = cam;
  598. urb->pipe = usb_rcvisocpipe(cam->dev, 1 /*ISOC endpoint*/);
  599. urb->transfer_flags = URB_ISO_ASAP;
  600. urb->transfer_buffer = cam->sbuf[i].data;
  601. urb->complete = cpia2_usb_complete;
  602. urb->number_of_packets = FRAMES_PER_DESC;
  603. urb->interval = 1;
  604. urb->transfer_buffer_length =
  605. FRAME_SIZE_PER_DESC * FRAMES_PER_DESC;
  606. for (fx = 0; fx < FRAMES_PER_DESC; fx++) {
  607. urb->iso_frame_desc[fx].offset =
  608. FRAME_SIZE_PER_DESC * fx;
  609. urb->iso_frame_desc[fx].length = FRAME_SIZE_PER_DESC;
  610. }
  611. }
  612. /* Queue the ISO urbs, and resubmit in the completion handler */
  613. for(i=0; i<NUM_SBUF; ++i) {
  614. err = usb_submit_urb(cam->sbuf[i].urb, GFP_KERNEL);
  615. if (err) {
  616. ERR("usb_submit_urb[%d]() = %d\n", i, err);
  617. return err;
  618. }
  619. }
  620. return 0;
  621. }
  622. /******************************************************************************
  623. *
  624. * cpia2_usb_stream_start
  625. *
  626. *****************************************************************************/
  627. int cpia2_usb_stream_start(struct camera_data *cam, unsigned int alternate)
  628. {
  629. int ret;
  630. int old_alt;
  631. if(cam->streaming)
  632. return 0;
  633. if (cam->flush) {
  634. int i;
  635. DBG("Flushing buffers\n");
  636. for(i=0; i<cam->num_frames; ++i) {
  637. cam->buffers[i].status = FRAME_EMPTY;
  638. cam->buffers[i].length = 0;
  639. }
  640. cam->curbuff = &cam->buffers[0];
  641. cam->workbuff = cam->curbuff->next;
  642. cam->flush = false;
  643. }
  644. old_alt = cam->params.camera_state.stream_mode;
  645. cam->params.camera_state.stream_mode = 0;
  646. ret = cpia2_usb_change_streaming_alternate(cam, alternate);
  647. if (ret < 0) {
  648. int ret2;
  649. ERR("cpia2_usb_change_streaming_alternate() = %d!\n", ret);
  650. cam->params.camera_state.stream_mode = old_alt;
  651. ret2 = set_alternate(cam, USBIF_CMDONLY);
  652. if (ret2 < 0) {
  653. ERR("cpia2_usb_change_streaming_alternate(%d) =%d has already "
  654. "failed. Then tried to call "
  655. "set_alternate(USBIF_CMDONLY) = %d.\n",
  656. alternate, ret, ret2);
  657. }
  658. } else {
  659. cam->frame_count = 0;
  660. cam->streaming = 1;
  661. ret = cpia2_usb_stream_resume(cam);
  662. }
  663. return ret;
  664. }
  665. /******************************************************************************
  666. *
  667. * cpia2_usb_stream_pause
  668. *
  669. *****************************************************************************/
  670. int cpia2_usb_stream_pause(struct camera_data *cam)
  671. {
  672. int ret = 0;
  673. if(cam->streaming) {
  674. free_sbufs(cam);
  675. ret = set_alternate(cam, USBIF_CMDONLY);
  676. }
  677. return ret;
  678. }
  679. /******************************************************************************
  680. *
  681. * cpia2_usb_stream_resume
  682. *
  683. *****************************************************************************/
  684. int cpia2_usb_stream_resume(struct camera_data *cam)
  685. {
  686. int ret = 0;
  687. if(cam->streaming) {
  688. cam->first_image_seen = 0;
  689. ret = set_alternate(cam, cam->params.camera_state.stream_mode);
  690. if(ret == 0) {
  691. /* for some reason the user effects need to be set
  692. again when starting streaming. */
  693. cpia2_do_command(cam, CPIA2_CMD_SET_USER_EFFECTS, TRANSFER_WRITE,
  694. cam->params.vp_params.user_effects);
  695. ret = submit_urbs(cam);
  696. }
  697. }
  698. return ret;
  699. }
  700. /******************************************************************************
  701. *
  702. * cpia2_usb_stream_stop
  703. *
  704. *****************************************************************************/
  705. int cpia2_usb_stream_stop(struct camera_data *cam)
  706. {
  707. int ret;
  708. ret = cpia2_usb_stream_pause(cam);
  709. cam->streaming = 0;
  710. configure_transfer_mode(cam, 0);
  711. return ret;
  712. }
  713. /******************************************************************************
  714. *
  715. * cpia2_usb_probe
  716. *
  717. * Probe and initialize.
  718. *****************************************************************************/
  719. static int cpia2_usb_probe(struct usb_interface *intf,
  720. const struct usb_device_id *id)
  721. {
  722. struct usb_device *udev = interface_to_usbdev(intf);
  723. struct usb_interface_descriptor *interface;
  724. struct camera_data *cam;
  725. int ret;
  726. /* A multi-config CPiA2 camera? */
  727. if (udev->descriptor.bNumConfigurations != 1)
  728. return -ENODEV;
  729. interface = &intf->cur_altsetting->desc;
  730. /* If we get to this point, we found a CPiA2 camera */
  731. LOG("CPiA2 USB camera found\n");
  732. cam = cpia2_init_camera_struct(intf);
  733. if (cam == NULL)
  734. return -ENOMEM;
  735. cam->dev = udev;
  736. cam->iface = interface->bInterfaceNumber;
  737. ret = set_alternate(cam, USBIF_CMDONLY);
  738. if (ret < 0) {
  739. ERR("%s: usb_set_interface error (ret = %d)\n", __func__, ret);
  740. kfree(cam);
  741. return ret;
  742. }
  743. if((ret = cpia2_init_camera(cam)) < 0) {
  744. ERR("%s: failed to initialize cpia2 camera (ret = %d)\n", __func__, ret);
  745. kfree(cam);
  746. return ret;
  747. }
  748. LOG(" CPiA Version: %d.%02d (%d.%d)\n",
  749. cam->params.version.firmware_revision_hi,
  750. cam->params.version.firmware_revision_lo,
  751. cam->params.version.asic_id,
  752. cam->params.version.asic_rev);
  753. LOG(" CPiA PnP-ID: %04x:%04x:%04x\n",
  754. cam->params.pnp_id.vendor,
  755. cam->params.pnp_id.product,
  756. cam->params.pnp_id.device_revision);
  757. LOG(" SensorID: %d.(version %d)\n",
  758. cam->params.version.sensor_flags,
  759. cam->params.version.sensor_rev);
  760. usb_set_intfdata(intf, cam);
  761. ret = cpia2_register_camera(cam);
  762. if (ret < 0) {
  763. ERR("%s: Failed to register cpia2 camera (ret = %d)\n", __func__, ret);
  764. kfree(cam);
  765. return ret;
  766. }
  767. return 0;
  768. }
  769. /******************************************************************************
  770. *
  771. * cpia2_disconnect
  772. *
  773. *****************************************************************************/
  774. static void cpia2_usb_disconnect(struct usb_interface *intf)
  775. {
  776. struct camera_data *cam = usb_get_intfdata(intf);
  777. usb_set_intfdata(intf, NULL);
  778. DBG("Stopping stream\n");
  779. cpia2_usb_stream_stop(cam);
  780. mutex_lock(&cam->v4l2_lock);
  781. DBG("Unregistering camera\n");
  782. cpia2_unregister_camera(cam);
  783. v4l2_device_disconnect(&cam->v4l2_dev);
  784. mutex_unlock(&cam->v4l2_lock);
  785. if(cam->buffers) {
  786. DBG("Wakeup waiting processes\n");
  787. cam->curbuff->status = FRAME_READY;
  788. cam->curbuff->length = 0;
  789. if (waitqueue_active(&cam->wq_stream))
  790. wake_up_interruptible(&cam->wq_stream);
  791. }
  792. DBG("Releasing interface\n");
  793. usb_driver_release_interface(&cpia2_driver, intf);
  794. v4l2_device_put(&cam->v4l2_dev);
  795. LOG("CPiA2 camera disconnected.\n");
  796. }
  797. static int cpia2_usb_suspend(struct usb_interface *intf, pm_message_t message)
  798. {
  799. struct camera_data *cam = usb_get_intfdata(intf);
  800. mutex_lock(&cam->v4l2_lock);
  801. if (cam->streaming) {
  802. cpia2_usb_stream_stop(cam);
  803. cam->streaming = 1;
  804. }
  805. mutex_unlock(&cam->v4l2_lock);
  806. dev_info(&intf->dev, "going into suspend..\n");
  807. return 0;
  808. }
  809. /* Resume device - start device. */
  810. static int cpia2_usb_resume(struct usb_interface *intf)
  811. {
  812. struct camera_data *cam = usb_get_intfdata(intf);
  813. mutex_lock(&cam->v4l2_lock);
  814. v4l2_ctrl_handler_setup(&cam->hdl);
  815. if (cam->streaming) {
  816. cam->streaming = 0;
  817. cpia2_usb_stream_start(cam,
  818. cam->params.camera_state.stream_mode);
  819. }
  820. mutex_unlock(&cam->v4l2_lock);
  821. dev_info(&intf->dev, "coming out of suspend..\n");
  822. return 0;
  823. }
  824. /******************************************************************************
  825. *
  826. * usb_cpia2_init
  827. *
  828. *****************************************************************************/
  829. int cpia2_usb_init(void)
  830. {
  831. return usb_register(&cpia2_driver);
  832. }
  833. /******************************************************************************
  834. *
  835. * usb_cpia_cleanup
  836. *
  837. *****************************************************************************/
  838. void cpia2_usb_cleanup(void)
  839. {
  840. schedule_timeout(2 * HZ);
  841. usb_deregister(&cpia2_driver);
  842. }