Guest User

Untitled

a guest
Jun 21st, 2020
121
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C 15.60 KB | None | 0 0
  1. #include <fcntl.h>
  2. #include <libusb.h>
  3. #include <stdint.h>
  4. #include <stdio.h>
  5. #include <stdlib.h>
  6. #include <string.h>
  7. #include <unistd.h>
  8.  
  9. #define TIMEOUT 500
  10.  
  11. /*
  12.  
  13. >cat /etc/udev/rules.d/50-usb.rules
  14. ...................................
  15. # Sony EyeToy
  16. SUBSYSTEM=="usb", ATTRS{idVendor}=="054c", ATTRS{idProduct}=="0155", MODE="0666"
  17.  
  18. >cat /etc/modprobe.d/blacklist.conf
  19. ...................................
  20. # Sony EyeToy
  21. blacklist gspca_ov519
  22. blacklist snd-usb-audio
  23.  
  24. >update-initramfs -u
  25.  
  26. >reboot
  27.  
  28. */
  29.  
  30.  
  31. // int libusb_control_transfer2(void* dev_handle,
  32. //  uint8_t     bmRequestType,
  33. //  uint8_t     bRequest,
  34. //  uint16_t        wValue,
  35. //  uint16_t        wIndex,
  36. //  unsigned char*  data,
  37. //  uint16_t        wLength,
  38. //  unsigned int    timeout) {
  39. //  printf("control = req:%02x, val:%02x, idx:%02x, len:%02x, data: ",
  40. //              bmRequestType << 8 | bRequest, wValue, wIndex, wLength);
  41. //  for (int i = 0; i < wLength; i++) {
  42. //      printf("%02x ", data[i]);
  43. //  }
  44. //  printf("\n");
  45. // }
  46.  
  47. int reg_write(void* dev_handle, int reg, int val) {
  48.     uint8_t data = val;
  49.     int length = sizeof(data);
  50.     int ret = libusb_control_transfer(dev_handle, 0x40, 0x01, 0x00, reg, &data, length, TIMEOUT);
  51.     if (ret != length) {
  52.         printf("%s:%d ret=%d(%s)\n", __FUNCTION__, __LINE__, ret, libusb_error_name(ret));
  53.         exit(1);
  54.     }
  55. }
  56.  
  57. int reg_read(void* dev_handle, int reg, int def) {
  58.     uint8_t data = def;
  59.     int length = sizeof(data);
  60.     int ret = libusb_control_transfer(dev_handle, 0xc0, 0x01, 0x00, reg, &data, length, TIMEOUT);
  61.     if (ret != length) {
  62.         printf("%s:%d ret=%d(%s)\n", __FUNCTION__, __LINE__, ret, libusb_error_name(ret));
  63.         exit(1);
  64.     }
  65.     return data;
  66. }
  67.  
  68. int i2c_write(void* dev_handle, int reg, int val) {
  69.     reg_write(dev_handle, 0x42, reg);
  70.     reg_write(dev_handle, 0x45, val);
  71.     reg_write(dev_handle, 0x47, 0x01);
  72. }
  73.  
  74. int i2c_read(void* dev_handle, int reg, int def) {
  75.     uint8_t data = def;
  76.     reg_write(dev_handle, 0x43, reg);
  77.     reg_write(dev_handle, 0x47, 0x03);
  78.     reg_write(dev_handle, 0x47, 0x05);
  79.     reg_read(dev_handle, 0x45, def);
  80.     return data;
  81. }
  82.  
  83. int eyetoy1_init_3(void* dev_handle, int arg1, int arg2, int arg3, int arg4, int arg5) {
  84.     reg_write(dev_handle, 0x16, 0x00); // DIVIDER - Vertical Divide by 1 / Horizontal Divide by 1
  85.    
  86.         i2c_write(dev_handle, 0x24, 0x30); // AEW  - AGC/AEC - Stable Operating Region - Upper Limit
  87.         i2c_write(dev_handle, 0x25, 0x7a); // AEB  - AGC/AEC - Stable Operating Region - Lower Limit
  88.         i2c_write(dev_handle, 0x67, 0x14); // RSVD - Reserved
  89.         i2c_write(dev_handle, 0x74, 0x20); // COMM - Common Mode Control M - default
  90.    
  91.     reg_write(dev_handle, 0x59, arg1); // CAMERA_CLOCK - Camera Clock Divider - CCLK = 48MHz/CAMERA_CLOCK
  92.  
  93.         i2c_write(dev_handle, 0x28, arg2); // COMH - Common Control H
  94.         i2c_write(dev_handle, 0x2d, arg3); // COMJ - Common Control J (bit4 = Auto-brightness)
  95.         i2c_write(dev_handle, 0x11, 0x00); // CLKRC - Data Format and Internal Clock - default
  96.    
  97.     reg_write(dev_handle, 0xa2, 0x20); // Reserved
  98.     reg_write(dev_handle, 0xa3, 0x20); // Reserved
  99.     reg_write(dev_handle, 0xe1, 0x30); // Reserved
  100.     reg_write(dev_handle, 0xe2, 0x78); // Reserved
  101.     reg_write(dev_handle, 0xa4, 0x04); // Reserved (Compression level)
  102.     reg_write(dev_handle, 0xa5, 0x20); // Reserved (Compression level)
  103.     reg_write(dev_handle, 0xe0, 0x00); // Reserved
  104.     reg_write(dev_handle, 0xe3, 0xef); // Reserved
  105.     reg_write(dev_handle, 0xe4, 0x82); // Reserved
  106.     reg_write(dev_handle, 0xe5, 0x82); // Reserved
  107.     reg_write(dev_handle, 0xe7, 0x04); // Reserved
  108.     reg_write(dev_handle, 0xe8, 0x04); // Reserved
  109.  
  110.         i2c_write(dev_handle, 0x2a, arg4); // FRARH - Output Format - Frame Rate Adjust High
  111.         i2c_write(dev_handle, 0x2b, arg5); // FRARL - Data Format - Frame Rate Adjust Setting LSB
  112.         i2c_write(dev_handle, 0x2a, arg4); // FRARH - Output Format - Frame Rate Adjust High
  113.         i2c_write(dev_handle, 0x2b, arg5); // FRARL - Data Format - Frame Rate Adjust Setting LSB
  114.  
  115.     reg_write(dev_handle, 0x51, 0x00); // RESET1 - Reset Control Register 0 - clear flags
  116.  
  117.     reg_write(dev_handle, 0x10, 0x14); // H_SIZE - Image Width  : 0x14*16 = 0x0140 (320)
  118.     reg_write(dev_handle, 0x11, 0x1e); // V_SIZE - Image Height : 0x1e* 8 = 0x00f0 (240)
  119.     reg_write(dev_handle, 0x12, 0x00); // X_OFFSETL - Windows top-left X coordinate (Low)
  120.     reg_write(dev_handle, 0x13, 0x00); // X_OFFSETH - Windows top-left X coordinate (High)
  121.     reg_write(dev_handle, 0x14, 0x00); // Y_OFFSETL - Windows top-left Y coordinate (Low)
  122.     reg_write(dev_handle, 0x15, 0x00); // Y_OFFSETH - Windows top-left Y coordinate (High)
  123.     reg_write(dev_handle, 0x25, 0x01); // Format[2:0] - Image format - YUV411
  124. }
  125.  
  126. int eyetoy1_init_2(void* dev_handle) {
  127.     reg_write(dev_handle, 0x5a, 0x6d); // YS_CTRL - System Control
  128.     reg_write(dev_handle, 0x53, 0xff); // EN_CLK0 - Clock Enable 0
  129.     reg_write(dev_handle, 0x54, 0xff); // EN_CLK1 - Clock Enable 1
  130.     reg_write(dev_handle, 0x5d, 0x03); // PWDN - Power-Down Control
  131.     reg_write(dev_handle, 0x51, 0x0f); // RESET1 - Reset Control Register 0 - FIFO+JPEG+SFIFO+CIF
  132.     reg_write(dev_handle, 0x51, 0x00); // RESET1 - Reset Control Register 0 - clear flags
  133.     reg_write(dev_handle, 0x5d, 0x03); // PWDN - Power-Down Control
  134.     reg_write(dev_handle, 0x53, 0x9f); // EN_CLK0 - Clock Enable 0
  135.     reg_write(dev_handle, 0x54, 0x0f); // EN_CLK1 - Clock Enable 1
  136.     reg_write(dev_handle, 0x37, 0x00); // Reserved
  137.     reg_write(dev_handle, 0x20, 0x0d); // DFR - Data Formatter - Swap U0<>V0, Swap Y0<>Y1, Swap Y0<>U0
  138.     reg_write(dev_handle, 0x21, 0x38); // SR - Syncronization Register - BufSync 8bit, BufSyncEnabl
  139.     reg_write(dev_handle, 0x55, 0x02); // AUDIO_CLK - Audio Clock Control -  Clock Select 2.048 MHz
  140.     reg_write(dev_handle, 0x22, 0x1d); // FRAR - Frame Rate Adjuster Register - YUV, EvenKeep, OddKeep, MaxFrameCount=5
  141.     reg_write(dev_handle, 0x17, 0x50); // Reserved
  142.     reg_write(dev_handle, 0x37, 0x01); // Reserved
  143.     reg_write(dev_handle, 0x40, 0xff); // Reserved
  144.     reg_write(dev_handle, 0x46, 0x00); // Reserved
  145.     reg_write(dev_handle, 0x48, 0x00); // Reserved
  146.     reg_write(dev_handle, 0x72, 0xee); // GPIO_IO_CTRL0 - GPIO Innput/Output Control0
  147.     reg_write(dev_handle, 0x71, 0x00); // GPIO_DATA_OUT0 - GPIO Output Data0 - [bit0]=Red LED
  148.     reg_write(dev_handle, 0x26, 0x00); // Reserved
  149.     reg_write(dev_handle, 0x23, 0xff); // Reserved
  150.     reg_write(dev_handle, 0x23, 0xff); // Reserved
  151.  
  152.     reg_write(dev_handle, 0x41, 0x42); // Reserved
  153.     reg_write(dev_handle, 0x44, 0x43); // Reserved
  154.  
  155.         i2c_write(dev_handle, 0x12, 0x80); // COMA - Common Control A - Reset all registers to default values
  156.  
  157.         i2c_read(dev_handle, 0x00, 0x00); // AGC - Gain control gain setting
  158.  
  159.         i2c_read(dev_handle, 0x1c, 0x7f); // MIDH - Manufacturer ID Byte – High
  160.         i2c_read(dev_handle, 0x1d, 0xa2); // MIDL - Manufacturer ID Byte – Low
  161.         i2c_read(dev_handle, 0x1c, 0x7f); // MIDH - Manufacturer ID Byte – High
  162.         i2c_read(dev_handle, 0x1d, 0xa2); // MIDL - Manufacturer ID Byte – Low
  163.  
  164.         i2c_write(dev_handle, 0x12, 0x80); // COMA - Common Control A - Reset all registers to default values
  165.         i2c_write(dev_handle, 0x12, 0x80); // COMA - Common Control A - Reset all registers to default values
  166.         i2c_write(dev_handle, 0x12, 0x80); // COMA - Common Control A - Reset all registers to default values
  167.  
  168.         i2c_write(dev_handle, 0x03, 0xa4); // SAT - Image Format – Color saturation value
  169.         i2c_write(dev_handle, 0x04, 0x30); // HUE - Image Format – Color hue control
  170.         i2c_write(dev_handle, 0x05, 0x88); // CWF - AWB – Red/Blue Pre-Amplifier gain setting
  171.         i2c_write(dev_handle, 0x06, 0x60); // BRT - ABC – Brightness setting
  172.         i2c_write(dev_handle, 0x11, 0x00); // CLKRC - Data Format and Internal Clock
  173.         i2c_write(dev_handle, 0x12, 0x05); // COMA - Common Control A - AWB-Enable + Reserved
  174.  
  175.         i2c_read(dev_handle, 0x00, 0x00); // AGC - Gain control gain setting
  176.  
  177.         i2c_write(dev_handle, 0x00, 0x00); // AGC - Gain control gain setting
  178.  
  179.         i2c_write(dev_handle, 0x10, 0x82); // AECH - Exposure Value
  180.         i2c_write(dev_handle, 0x13, 0xa3); // COMB - Common Control B - default
  181.         i2c_write(dev_handle, 0x14, 0x24); // COMC - Common Control C - QVGA
  182.         i2c_write(dev_handle, 0x15, 0x14); // COMD - Common Control D - Reserved
  183.         i2c_write(dev_handle, 0x1f, 0x41); // FACT - Output Format - Format Control - Reserved
  184.         i2c_write(dev_handle, 0x23, 0xde); // RSVD - Reserved
  185.         i2c_write(dev_handle, 0x26, 0xa2); // COMF - Common Control F - default
  186.         i2c_write(dev_handle, 0x27, 0xe2); // COMG - Common Control G - default
  187.         i2c_write(dev_handle, 0x28, 0x20); // COMH - Common Control H - Output Format - Progressive
  188.         i2c_write(dev_handle, 0x2f, 0x9d); // RSVD - Reserved
  189.         i2c_write(dev_handle, 0x30, 0x00); // RSVD - Reserved
  190.         i2c_write(dev_handle, 0x31, 0xc4); // RSVD - Reserved
  191.         i2c_write(dev_handle, 0x60, 0xa6); // SPCB - Signal Process Control B - AGC + Reserved
  192.         i2c_write(dev_handle, 0x61, 0xe0); // RSVD - Reserved
  193.         i2c_write(dev_handle, 0x62, 0x88); // RSVD - Reserved
  194.         i2c_write(dev_handle, 0x63, 0x11); // RSVD - Reserved
  195.         i2c_write(dev_handle, 0x64, 0x89); // RSVD - Reserved
  196.         i2c_write(dev_handle, 0x65, 0x00); // RSVD - Reserved
  197.         i2c_write(dev_handle, 0x67, 0x94); // RSVD - Reserved
  198.         i2c_write(dev_handle, 0x68, 0x7a); // RSVD - Reserved (bit 0:2 = Audio gain)
  199.         i2c_write(dev_handle, 0x69, 0x08); // RSVD - Reserved
  200.         i2c_write(dev_handle, 0x6c, 0x11); // RMCO - Color Matrix - RGB Crosstalk Compensation - R Channel
  201.         i2c_write(dev_handle, 0x6d, 0x33); // GMCO - Color Matrix - RGB Crosstalk Compensation - G Channel
  202.         i2c_write(dev_handle, 0x6e, 0x22); // BMCO - Color Matrix - RGB Crosstalk Compensation - B Channel
  203.         i2c_write(dev_handle, 0x6f, 0x00); // RSVD - Reserved
  204.         i2c_write(dev_handle, 0x74, 0x20); // COMM - Common Mode Control M - default
  205.         i2c_write(dev_handle, 0x75, 0x06); // COMN - Common Mode Control N - Reserved
  206.         i2c_write(dev_handle, 0x77, 0xc4); // RSVD - Reserved
  207.  
  208.         i2c_write(dev_handle, 0x14, 0x24); // COMC - Common Control C - QVGA
  209.         i2c_write(dev_handle, 0x26, 0xa2); // COMF - Common Control F - default
  210.  
  211.         i2c_write(dev_handle, 0x17, 0x1c); // HSTART - Output Format - Horizontal Frame (HREF Column) Start
  212.         i2c_write(dev_handle, 0x18, 0xbc); // HSTOP  - Output Format - Horizontal Frame (HREF Column) Stop
  213.    
  214.         i2c_write(dev_handle, 0x12, 0x45); // COMA - Common Control A - AWB-Enable + Reserved + Mirror Image Enable
  215.         i2c_write(dev_handle, 0x20, 0xd2); // COME - Common Control E - Reserved + Edge Enh + AEC + Reserved
  216.         i2c_write(dev_handle, 0x15, 0x00); // COMD - Common Control D - clear reserved flags
  217.  
  218.     eyetoy1_init_3(dev_handle, 0x00, 0x20, 0x01, 0x10, 0x00);
  219.  
  220.     reg_write(dev_handle, 0xa0, 0x42); // Reserved
  221.     reg_write(dev_handle, 0x51, 0x00); // RESET1 - Reset Control Register 0 - clear flags
  222. }
  223.  
  224. int eyetoy1_init_1(void* dev_handle) {
  225.     int ret = 0;
  226.     ret = libusb_set_configuration(dev_handle, 1);
  227.     if (ret != LIBUSB_SUCCESS) {
  228.         printf("%s:%d ret=%d(%s)\n", __FUNCTION__, __LINE__, ret, libusb_error_name(ret));
  229.         exit(1);
  230.     }
  231.  
  232.     ret = libusb_detach_kernel_driver(dev_handle, 0);
  233.     if (ret != LIBUSB_SUCCESS) {
  234.         printf("%s:%d ret=%d(%s)\n", __FUNCTION__, __LINE__, ret, libusb_error_name(ret));
  235.         // exit(1);
  236.     }
  237.     ret = libusb_claim_interface(dev_handle, 0);
  238.     if (ret != LIBUSB_SUCCESS) {
  239.         printf("%s:%d ret=%d(%s)\n", __FUNCTION__, __LINE__, ret, libusb_error_name(ret));
  240.         exit(1);
  241.     }
  242.     ret = libusb_set_interface_alt_setting(dev_handle, 0, 4);
  243.     if (ret != LIBUSB_SUCCESS) {
  244.         printf("%s:%d ret=%d(%s)\n", __FUNCTION__, __LINE__, ret, libusb_error_name(ret));
  245.         exit(1);
  246.     }
  247.  
  248.     ret = libusb_detach_kernel_driver(dev_handle, 2);
  249.     if (ret != LIBUSB_SUCCESS) {
  250.         printf("%s:%d ret=%d(%s)\n", __FUNCTION__, __LINE__, ret, libusb_error_name(ret));
  251.         // exit(1);
  252.     }
  253.     ret = libusb_claim_interface(dev_handle, 2);
  254.     if (ret != LIBUSB_SUCCESS) {
  255.         printf("%s:%d ret=%d(%s)\n", __FUNCTION__, __LINE__, ret, libusb_error_name(ret));
  256.         exit(1);
  257.     }
  258.     ret = libusb_set_interface_alt_setting(dev_handle, 2, 1);
  259.     if (ret != LIBUSB_SUCCESS) {
  260.         printf("%s:%d ret=%d(%s)\n", __FUNCTION__, __LINE__, ret, libusb_error_name(ret));
  261.         exit(1);
  262.     }
  263.  
  264.     eyetoy1_init_2(dev_handle);
  265.     eyetoy1_init_2(dev_handle);
  266.  
  267.     eyetoy1_init_3(dev_handle, 0x02, 0x00, 0x41, 0x90, 0x98);
  268. }
  269.  
  270. struct isoc_trans {
  271.     struct libusb_transfer *transfer_buff;
  272.     uint8_t isoc_buffer[896];
  273.     int is_pending_flag;
  274. };
  275.  
  276. void isoc_transfer_completion_handler(struct libusb_transfer *transfer) {
  277.     // printf("iso comp : %d/%d\n", transfer->length, transfer->actual_length);
  278.     unsigned char *b = transfer->buffer;
  279.     static int file_index = 0;
  280.     static int fd = -1;
  281.  
  282.     if (transfer->buffer[0] == 0xff
  283.      && transfer->buffer[1] == 0xff
  284.      && transfer->buffer[2] == 0xff
  285.      && transfer->buffer[3] == 0x50) {
  286.         char file_name[80] = {0};
  287.         sprintf(file_name, "dumps/file_%03d", file_index++);
  288.         printf("open file = %s\n", file_name);
  289.         fd = open(file_name, O_RDWR | O_CREAT );// | O_SYNC);
  290.  
  291.         for (int i = 0; i < transfer->length; i+=0x10) {
  292.             if (b[i+0]!=0xFF || b[i+1]!=0xFF || b[i+2]!=0xFF || b[i+3]!=0xFF
  293.              && b[i+4]!=0xFF || b[i+5]!=0xFF || b[i+6]!=0xFF || b[i+7]!=0xFF
  294.              && b[i+8]!=0xFF || b[i+9]!=0xFF || b[i+0xa]!=0xFF || b[i+0xb]!=0xFF
  295.              && b[i+0xc]!=0xFF || b[i+0xd]!=0xFF || b[i+0xe]!=0xFF || b[i+0xf]!=0xFF) {
  296.                 write(fd, b+i, 0x10);
  297.             }
  298.         }
  299.     } else if (transfer->buffer[0] == 0xff
  300.      && transfer->buffer[1] == 0xff
  301.      && transfer->buffer[2] == 0xff
  302.      && transfer->buffer[3] == 0x51) {
  303.         write(fd, b, 0x10);
  304.         close(fd);
  305.         fd = -1;
  306.     } else {
  307.         for (int i = 0; i < transfer->length; i+=0x10) {
  308.             if (b[i+0]!=0xFF || b[i+1]!=0xFF || b[i+2]!=0xFF || b[i+3]!=0xFF
  309.              && b[i+4]!=0xFF || b[i+5]!=0xFF || b[i+6]!=0xFF || b[i+7]!=0xFF
  310.              && b[i+8]!=0xFF || b[i+9]!=0xFF || b[i+0xa]!=0xFF || b[i+0xb]!=0xFF
  311.              && b[i+0xc]!=0xFF || b[i+0xd]!=0xFF || b[i+0xe]!=0xFF || b[i+0xf]!=0xFF) {
  312.                 write(fd, b+i, 0x10);
  313.             }
  314.         }
  315.     }
  316.  
  317.     if (transfer->user_data != NULL) {
  318.         struct isoc_trans *sp = (struct isoc_trans*) transfer->user_data;
  319.         sp->is_pending_flag = 0;
  320.     }
  321. }
  322.  
  323. int main(int argc, char **argv) {
  324.     libusb_context *ctx = NULL;
  325.     int ret = libusb_init(&ctx);
  326.     if (ret != LIBUSB_SUCCESS) {
  327.         printf("%s:%d ret=%d(%s)\n", __FUNCTION__, __LINE__, ret, libusb_error_name(ret));
  328.         exit(1);
  329.     }
  330.  
  331.     void* dev_handle = libusb_open_device_with_vid_pid(ctx, 0x054c, 0x0155);
  332.     if (dev_handle == NULL) {
  333.         printf("%s:%d ret=%p\n", __FUNCTION__, __LINE__, dev_handle);
  334.         exit(1);
  335.     }
  336.  
  337.     eyetoy1_init_1(dev_handle);
  338.  
  339.  
  340.     // int pk_len = 896;
  341.     int pk_len = 896;
  342.     int loop = 1000;
  343.  
  344.     struct isoc_trans transfer_array[50];
  345.     for(int i = 0; i < sizeof(transfer_array) / sizeof(transfer_array[0]); i++) {
  346.         transfer_array[i].is_pending_flag = 0;
  347.         transfer_array[i].transfer_buff = libusb_alloc_transfer(1);
  348.     }
  349.  
  350.     struct timeval poll_timeout;
  351.     poll_timeout.tv_sec = 0;
  352.     poll_timeout.tv_usec = 1000;
  353.  
  354.     while (loop --> 0) {
  355.         for(int i = 0; i < sizeof(transfer_array) / sizeof(transfer_array[0]); i++) {
  356.             if(transfer_array[i].is_pending_flag) {
  357.                 libusb_handle_events_timeout(NULL, &poll_timeout);
  358.                 break;
  359.             }
  360.         }
  361.         for(int i = 0; i < sizeof(transfer_array) / sizeof(transfer_array[0]); i++) {
  362.             if(transfer_array[i].is_pending_flag) {
  363.                 libusb_handle_events_timeout(NULL, &poll_timeout);
  364.                 printf("pending...\n");
  365.                 continue;
  366.             }
  367.             memset(transfer_array[i].transfer_buff, 0, sizeof(transfer_array[i].transfer_buff));
  368.  
  369.             libusb_fill_iso_transfer(
  370.                 transfer_array[i].transfer_buff,
  371.                 dev_handle,
  372.                 LIBUSB_ENDPOINT_IN | 1,
  373.                 transfer_array[i].isoc_buffer,
  374.                 pk_len,
  375.                 1,
  376.                 isoc_transfer_completion_handler,
  377.                 &transfer_array[i],
  378.                 1000);
  379.             libusb_set_iso_packet_lengths(transfer_array[i].transfer_buff, pk_len);
  380.  
  381.             transfer_array[i].is_pending_flag = 1;
  382.             ret = libusb_submit_transfer(transfer_array[i].transfer_buff);
  383.  
  384.             break;
  385.         }
  386.         libusb_handle_events(NULL);
  387.     }
  388. }
Advertisement
Add Comment
Please, Sign In to add comment