et61x251_core.c 64 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899900901902903904905906907908909910911912913914915916917918919920921922923924925926927928929930931932933934935936937938939940941942943944945946947948949950951952953954955956957958959960961962963964965966967968969970971972973974975976977978979980981982983984985986987988989990991992993994995996997998999100010011002100310041005100610071008100910101011101210131014101510161017101810191020102110221023102410251026102710281029103010311032103310341035103610371038103910401041104210431044104510461047104810491050105110521053105410551056105710581059106010611062106310641065106610671068106910701071107210731074107510761077107810791080108110821083108410851086108710881089109010911092109310941095109610971098109911001101110211031104110511061107110811091110111111121113111411151116111711181119112011211122112311241125112611271128112911301131113211331134113511361137113811391140114111421143114411451146114711481149115011511152115311541155115611571158115911601161116211631164116511661167116811691170117111721173117411751176117711781179118011811182118311841185118611871188118911901191119211931194119511961197119811991200120112021203120412051206120712081209121012111212121312141215121612171218121912201221122212231224122512261227122812291230123112321233123412351236123712381239124012411242124312441245124612471248124912501251125212531254125512561257125812591260126112621263126412651266126712681269127012711272127312741275127612771278127912801281128212831284128512861287128812891290129112921293129412951296129712981299130013011302130313041305130613071308130913101311131213131314131513161317131813191320132113221323132413251326132713281329133013311332133313341335133613371338133913401341134213431344134513461347134813491350135113521353135413551356135713581359136013611362136313641365136613671368136913701371137213731374137513761377137813791380138113821383138413851386138713881389139013911392139313941395139613971398139914001401140214031404140514061407140814091410141114121413141414151416141714181419142014211422142314241425142614271428142914301431143214331434143514361437143814391440144114421443144414451446144714481449145014511452145314541455145614571458145914601461146214631464146514661467146814691470147114721473147414751476147714781479148014811482148314841485148614871488148914901491149214931494149514961497149814991500150115021503150415051506150715081509151015111512151315141515151615171518151915201521152215231524152515261527152815291530153115321533153415351536153715381539154015411542154315441545154615471548154915501551155215531554155515561557155815591560156115621563156415651566156715681569157015711572157315741575157615771578157915801581158215831584158515861587158815891590159115921593159415951596159715981599160016011602160316041605160616071608160916101611161216131614161516161617161816191620162116221623162416251626162716281629163016311632163316341635163616371638163916401641164216431644164516461647164816491650165116521653165416551656165716581659166016611662166316641665166616671668166916701671167216731674167516761677167816791680168116821683168416851686168716881689169016911692169316941695169616971698169917001701170217031704170517061707170817091710171117121713171417151716171717181719172017211722172317241725172617271728172917301731173217331734173517361737173817391740174117421743174417451746174717481749175017511752175317541755175617571758175917601761176217631764176517661767176817691770177117721773177417751776177717781779178017811782178317841785178617871788178917901791179217931794179517961797179817991800180118021803180418051806180718081809181018111812181318141815181618171818181918201821182218231824182518261827182818291830183118321833183418351836183718381839184018411842184318441845184618471848184918501851185218531854185518561857185818591860186118621863186418651866186718681869187018711872187318741875187618771878187918801881188218831884188518861887188818891890189118921893189418951896189718981899190019011902190319041905190619071908190919101911191219131914191519161917191819191920192119221923192419251926192719281929193019311932193319341935193619371938193919401941194219431944194519461947194819491950195119521953195419551956195719581959196019611962196319641965196619671968196919701971197219731974197519761977197819791980198119821983198419851986198719881989199019911992199319941995199619971998199920002001200220032004200520062007200820092010201120122013201420152016201720182019202020212022202320242025202620272028202920302031203220332034203520362037203820392040204120422043204420452046204720482049205020512052205320542055205620572058205920602061206220632064206520662067206820692070207120722073207420752076207720782079208020812082208320842085208620872088208920902091209220932094209520962097209820992100210121022103210421052106210721082109211021112112211321142115211621172118211921202121212221232124212521262127212821292130213121322133213421352136213721382139214021412142214321442145214621472148214921502151215221532154215521562157215821592160216121622163216421652166216721682169217021712172217321742175217621772178217921802181218221832184218521862187218821892190219121922193219421952196219721982199220022012202220322042205220622072208220922102211221222132214221522162217221822192220222122222223222422252226222722282229223022312232223322342235223622372238223922402241224222432244224522462247224822492250225122522253225422552256225722582259226022612262226322642265226622672268226922702271227222732274227522762277227822792280228122822283228422852286228722882289229022912292229322942295229622972298229923002301230223032304230523062307230823092310231123122313231423152316231723182319232023212322232323242325232623272328232923302331233223332334233523362337233823392340234123422343234423452346234723482349235023512352235323542355235623572358235923602361236223632364236523662367236823692370237123722373237423752376237723782379238023812382238323842385238623872388238923902391239223932394239523962397239823992400240124022403240424052406240724082409241024112412241324142415241624172418241924202421242224232424242524262427242824292430243124322433243424352436243724382439244024412442244324442445244624472448244924502451245224532454245524562457245824592460246124622463246424652466246724682469247024712472247324742475247624772478247924802481248224832484248524862487248824892490249124922493249424952496249724982499250025012502250325042505250625072508250925102511251225132514251525162517251825192520252125222523252425252526252725282529253025312532253325342535253625372538253925402541254225432544254525462547254825492550255125522553255425552556255725582559256025612562256325642565256625672568256925702571257225732574257525762577257825792580258125822583258425852586258725882589259025912592259325942595259625972598259926002601260226032604260526062607260826092610261126122613261426152616261726182619262026212622262326242625262626272628262926302631263226332634263526362637263826392640264126422643264426452646264726482649265026512652265326542655265626572658265926602661266226632664266526662667266826692670267126722673267426752676267726782679268026812682268326842685268626872688268926902691269226932694269526962697269826992700270127022703
  1. /***************************************************************************
  2. * V4L2 driver for ET61X[12]51 PC Camera Controllers *
  3. * *
  4. * Copyright (C) 2006-2007 by Luca Risolia <luca.risolia@studio.unibo.it> *
  5. * *
  6. * This program is free software; you can redistribute it and/or modify *
  7. * it under the terms of the GNU General Public License as published by *
  8. * the Free Software Foundation; either version 2 of the License, or *
  9. * (at your option) any later version. *
  10. * *
  11. * This program is distributed in the hope that it will be useful, *
  12. * but WITHOUT ANY WARRANTY; without even the implied warranty of *
  13. * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
  14. * GNU General Public License for more details. *
  15. * *
  16. * You should have received a copy of the GNU General Public License *
  17. * along with this program; if not, write to the Free Software *
  18. * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. *
  19. ***************************************************************************/
  20. #include <linux/module.h>
  21. #include <linux/init.h>
  22. #include <linux/kernel.h>
  23. #include <linux/param.h>
  24. #include <linux/moduleparam.h>
  25. #include <linux/errno.h>
  26. #include <linux/slab.h>
  27. #include <linux/device.h>
  28. #include <linux/fs.h>
  29. #include <linux/delay.h>
  30. #include <linux/compiler.h>
  31. #include <linux/ioctl.h>
  32. #include <linux/poll.h>
  33. #include <linux/stat.h>
  34. #include <linux/mm.h>
  35. #include <linux/vmalloc.h>
  36. #include <linux/page-flags.h>
  37. #include <linux/byteorder/generic.h>
  38. #include <asm/page.h>
  39. #include <asm/uaccess.h>
  40. #include "et61x251.h"
  41. /*****************************************************************************/
  42. #define ET61X251_MODULE_NAME "V4L2 driver for ET61X[12]51 " \
  43. "PC Camera Controllers"
  44. #define ET61X251_MODULE_AUTHOR "(C) 2006-2007 Luca Risolia"
  45. #define ET61X251_AUTHOR_EMAIL "<luca.risolia@studio.unibo.it>"
  46. #define ET61X251_MODULE_LICENSE "GPL"
  47. #define ET61X251_MODULE_VERSION "1:1.09"
  48. #define ET61X251_MODULE_VERSION_CODE KERNEL_VERSION(1, 1, 9)
  49. /*****************************************************************************/
  50. MODULE_DEVICE_TABLE(usb, et61x251_id_table);
  51. MODULE_AUTHOR(ET61X251_MODULE_AUTHOR " " ET61X251_AUTHOR_EMAIL);
  52. MODULE_DESCRIPTION(ET61X251_MODULE_NAME);
  53. MODULE_VERSION(ET61X251_MODULE_VERSION);
  54. MODULE_LICENSE(ET61X251_MODULE_LICENSE);
  55. static short video_nr[] = {[0 ... ET61X251_MAX_DEVICES-1] = -1};
  56. module_param_array(video_nr, short, NULL, 0444);
  57. MODULE_PARM_DESC(video_nr,
  58. "\n<-1|n[,...]> Specify V4L2 minor mode number."
  59. "\n -1 = use next available (default)"
  60. "\n n = use minor number n (integer >= 0)"
  61. "\nYou can specify up to "
  62. __MODULE_STRING(ET61X251_MAX_DEVICES) " cameras this way."
  63. "\nFor example:"
  64. "\nvideo_nr=-1,2,-1 would assign minor number 2 to"
  65. "\nthe second registered camera and use auto for the first"
  66. "\none and for every other camera."
  67. "\n");
  68. static short force_munmap[] = {[0 ... ET61X251_MAX_DEVICES-1] =
  69. ET61X251_FORCE_MUNMAP};
  70. module_param_array(force_munmap, bool, NULL, 0444);
  71. MODULE_PARM_DESC(force_munmap,
  72. "\n<0|1[,...]> Force the application to unmap previously"
  73. "\nmapped buffer memory before calling any VIDIOC_S_CROP or"
  74. "\nVIDIOC_S_FMT ioctl's. Not all the applications support"
  75. "\nthis feature. This parameter is specific for each"
  76. "\ndetected camera."
  77. "\n 0 = do not force memory unmapping"
  78. "\n 1 = force memory unmapping (save memory)"
  79. "\nDefault value is "__MODULE_STRING(ET61X251_FORCE_MUNMAP)"."
  80. "\n");
  81. static unsigned int frame_timeout[] = {[0 ... ET61X251_MAX_DEVICES-1] =
  82. ET61X251_FRAME_TIMEOUT};
  83. module_param_array(frame_timeout, uint, NULL, 0644);
  84. MODULE_PARM_DESC(frame_timeout,
  85. "\n<n[,...]> Timeout for a video frame in seconds."
  86. "\nThis parameter is specific for each detected camera."
  87. "\nDefault value is "
  88. __MODULE_STRING(ET61X251_FRAME_TIMEOUT)"."
  89. "\n");
  90. #ifdef ET61X251_DEBUG
  91. static unsigned short debug = ET61X251_DEBUG_LEVEL;
  92. module_param(debug, ushort, 0644);
  93. MODULE_PARM_DESC(debug,
  94. "\n<n> Debugging information level, from 0 to 3:"
  95. "\n0 = none (use carefully)"
  96. "\n1 = critical errors"
  97. "\n2 = significant informations"
  98. "\n3 = more verbose messages"
  99. "\nLevel 3 is useful for testing only, when only "
  100. "one device is used."
  101. "\nDefault value is "__MODULE_STRING(ET61X251_DEBUG_LEVEL)"."
  102. "\n");
  103. #endif
  104. /*****************************************************************************/
  105. static u32
  106. et61x251_request_buffers(struct et61x251_device* cam, u32 count,
  107. enum et61x251_io_method io)
  108. {
  109. struct v4l2_pix_format* p = &(cam->sensor.pix_format);
  110. struct v4l2_rect* r = &(cam->sensor.cropcap.bounds);
  111. const size_t imagesize = cam->module_param.force_munmap ||
  112. io == IO_READ ?
  113. (p->width * p->height * p->priv) / 8 :
  114. (r->width * r->height * p->priv) / 8;
  115. void* buff = NULL;
  116. u32 i;
  117. if (count > ET61X251_MAX_FRAMES)
  118. count = ET61X251_MAX_FRAMES;
  119. cam->nbuffers = count;
  120. while (cam->nbuffers > 0) {
  121. if ((buff = vmalloc_32_user(cam->nbuffers *
  122. PAGE_ALIGN(imagesize))))
  123. break;
  124. cam->nbuffers--;
  125. }
  126. for (i = 0; i < cam->nbuffers; i++) {
  127. cam->frame[i].bufmem = buff + i*PAGE_ALIGN(imagesize);
  128. cam->frame[i].buf.index = i;
  129. cam->frame[i].buf.m.offset = i*PAGE_ALIGN(imagesize);
  130. cam->frame[i].buf.length = imagesize;
  131. cam->frame[i].buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  132. cam->frame[i].buf.sequence = 0;
  133. cam->frame[i].buf.field = V4L2_FIELD_NONE;
  134. cam->frame[i].buf.memory = V4L2_MEMORY_MMAP;
  135. cam->frame[i].buf.flags = 0;
  136. }
  137. return cam->nbuffers;
  138. }
  139. static void et61x251_release_buffers(struct et61x251_device* cam)
  140. {
  141. if (cam->nbuffers) {
  142. vfree(cam->frame[0].bufmem);
  143. cam->nbuffers = 0;
  144. }
  145. cam->frame_current = NULL;
  146. }
  147. static void et61x251_empty_framequeues(struct et61x251_device* cam)
  148. {
  149. u32 i;
  150. INIT_LIST_HEAD(&cam->inqueue);
  151. INIT_LIST_HEAD(&cam->outqueue);
  152. for (i = 0; i < ET61X251_MAX_FRAMES; i++) {
  153. cam->frame[i].state = F_UNUSED;
  154. cam->frame[i].buf.bytesused = 0;
  155. }
  156. }
  157. static void et61x251_requeue_outqueue(struct et61x251_device* cam)
  158. {
  159. struct et61x251_frame_t *i;
  160. list_for_each_entry(i, &cam->outqueue, frame) {
  161. i->state = F_QUEUED;
  162. list_add(&i->frame, &cam->inqueue);
  163. }
  164. INIT_LIST_HEAD(&cam->outqueue);
  165. }
  166. static void et61x251_queue_unusedframes(struct et61x251_device* cam)
  167. {
  168. unsigned long lock_flags;
  169. u32 i;
  170. for (i = 0; i < cam->nbuffers; i++)
  171. if (cam->frame[i].state == F_UNUSED) {
  172. cam->frame[i].state = F_QUEUED;
  173. spin_lock_irqsave(&cam->queue_lock, lock_flags);
  174. list_add_tail(&cam->frame[i].frame, &cam->inqueue);
  175. spin_unlock_irqrestore(&cam->queue_lock, lock_flags);
  176. }
  177. }
  178. /*****************************************************************************/
  179. int et61x251_write_reg(struct et61x251_device* cam, u8 value, u16 index)
  180. {
  181. struct usb_device* udev = cam->usbdev;
  182. u8* buff = cam->control_buffer;
  183. int res;
  184. *buff = value;
  185. res = usb_control_msg(udev, usb_sndctrlpipe(udev, 0), 0x00, 0x41,
  186. 0, index, buff, 1, ET61X251_CTRL_TIMEOUT);
  187. if (res < 0) {
  188. DBG(3, "Failed to write a register (value 0x%02X, index "
  189. "0x%02X, error %d)", value, index, res);
  190. return -1;
  191. }
  192. return 0;
  193. }
  194. int et61x251_read_reg(struct et61x251_device* cam, u16 index)
  195. {
  196. struct usb_device* udev = cam->usbdev;
  197. u8* buff = cam->control_buffer;
  198. int res;
  199. res = usb_control_msg(udev, usb_rcvctrlpipe(udev, 0), 0x00, 0xc1,
  200. 0, index, buff, 1, ET61X251_CTRL_TIMEOUT);
  201. if (res < 0)
  202. DBG(3, "Failed to read a register (index 0x%02X, error %d)",
  203. index, res);
  204. return (res >= 0) ? (int)(*buff) : -1;
  205. }
  206. static int
  207. et61x251_i2c_wait(struct et61x251_device* cam,
  208. const struct et61x251_sensor* sensor)
  209. {
  210. int i, r;
  211. for (i = 1; i <= 8; i++) {
  212. if (sensor->interface == ET61X251_I2C_3WIRES) {
  213. r = et61x251_read_reg(cam, 0x8e);
  214. if (!(r & 0x02) && (r >= 0))
  215. return 0;
  216. } else {
  217. r = et61x251_read_reg(cam, 0x8b);
  218. if (!(r & 0x01) && (r >= 0))
  219. return 0;
  220. }
  221. if (r < 0)
  222. return -EIO;
  223. udelay(8*8); /* minimum for sensors at 400kHz */
  224. }
  225. return -EBUSY;
  226. }
  227. int
  228. et61x251_i2c_try_read(struct et61x251_device* cam,
  229. const struct et61x251_sensor* sensor, u8 address)
  230. {
  231. struct usb_device* udev = cam->usbdev;
  232. u8* data = cam->control_buffer;
  233. int err = 0, res;
  234. data[0] = address;
  235. data[1] = cam->sensor.i2c_slave_id;
  236. data[2] = cam->sensor.rsta | 0x10;
  237. data[3] = !(et61x251_read_reg(cam, 0x8b) & 0x02);
  238. res = usb_control_msg(udev, usb_sndctrlpipe(udev, 0), 0x00, 0x41,
  239. 0, 0x88, data, 4, ET61X251_CTRL_TIMEOUT);
  240. if (res < 0)
  241. err += res;
  242. err += et61x251_i2c_wait(cam, sensor);
  243. res = usb_control_msg(udev, usb_rcvctrlpipe(udev, 0), 0x00, 0xc1,
  244. 0, 0x80, data, 8, ET61X251_CTRL_TIMEOUT);
  245. if (res < 0)
  246. err += res;
  247. if (err)
  248. DBG(3, "I2C read failed for %s image sensor", sensor->name);
  249. PDBGG("I2C read: address 0x%02X, value: 0x%02X", address, data[0]);
  250. return err ? -1 : (int)data[0];
  251. }
  252. int
  253. et61x251_i2c_try_write(struct et61x251_device* cam,
  254. const struct et61x251_sensor* sensor, u8 address,
  255. u8 value)
  256. {
  257. struct usb_device* udev = cam->usbdev;
  258. u8* data = cam->control_buffer;
  259. int err = 0, res;
  260. data[0] = address;
  261. data[1] = cam->sensor.i2c_slave_id;
  262. data[2] = cam->sensor.rsta | 0x12;
  263. res = usb_control_msg(udev, usb_sndctrlpipe(udev, 0), 0x00, 0x41,
  264. 0, 0x88, data, 3, ET61X251_CTRL_TIMEOUT);
  265. if (res < 0)
  266. err += res;
  267. data[0] = value;
  268. res = usb_control_msg(udev, usb_sndctrlpipe(udev, 0), 0x00, 0x41,
  269. 0, 0x80, data, 1, ET61X251_CTRL_TIMEOUT);
  270. if (res < 0)
  271. err += res;
  272. err += et61x251_i2c_wait(cam, sensor);
  273. if (err)
  274. DBG(3, "I2C write failed for %s image sensor", sensor->name);
  275. PDBGG("I2C write: address 0x%02X, value: 0x%02X", address, value);
  276. return err ? -1 : 0;
  277. }
  278. int
  279. et61x251_i2c_raw_write(struct et61x251_device* cam, u8 n, u8 data1, u8 data2,
  280. u8 data3, u8 data4, u8 data5, u8 data6, u8 data7,
  281. u8 data8, u8 address)
  282. {
  283. struct usb_device* udev = cam->usbdev;
  284. u8* data = cam->control_buffer;
  285. int err = 0, res;
  286. data[0] = data2;
  287. data[1] = data3;
  288. data[2] = data4;
  289. data[3] = data5;
  290. data[4] = data6;
  291. data[5] = data7;
  292. data[6] = data8;
  293. res = usb_control_msg(udev, usb_sndctrlpipe(udev, 0), 0x00, 0x41,
  294. 0, 0x81, data, n-1, ET61X251_CTRL_TIMEOUT);
  295. if (res < 0)
  296. err += res;
  297. data[0] = address;
  298. data[1] = cam->sensor.i2c_slave_id;
  299. data[2] = cam->sensor.rsta | 0x02 | (n << 4);
  300. res = usb_control_msg(udev, usb_sndctrlpipe(udev, 0), 0x00, 0x41,
  301. 0, 0x88, data, 3, ET61X251_CTRL_TIMEOUT);
  302. if (res < 0)
  303. err += res;
  304. /* Start writing through the serial interface */
  305. data[0] = data1;
  306. res = usb_control_msg(udev, usb_sndctrlpipe(udev, 0), 0x00, 0x41,
  307. 0, 0x80, data, 1, ET61X251_CTRL_TIMEOUT);
  308. if (res < 0)
  309. err += res;
  310. err += et61x251_i2c_wait(cam, &cam->sensor);
  311. if (err)
  312. DBG(3, "I2C raw write failed for %s image sensor",
  313. cam->sensor.name);
  314. PDBGG("I2C raw write: %u bytes, address = 0x%02X, data1 = 0x%02X, "
  315. "data2 = 0x%02X, data3 = 0x%02X, data4 = 0x%02X, data5 = 0x%02X,"
  316. " data6 = 0x%02X, data7 = 0x%02X, data8 = 0x%02X", n, address,
  317. data1, data2, data3, data4, data5, data6, data7, data8);
  318. return err ? -1 : 0;
  319. }
  320. int et61x251_i2c_read(struct et61x251_device* cam, u8 address)
  321. {
  322. return et61x251_i2c_try_read(cam, &cam->sensor, address);
  323. }
  324. int et61x251_i2c_write(struct et61x251_device* cam, u8 address, u8 value)
  325. {
  326. return et61x251_i2c_try_write(cam, &cam->sensor, address, value);
  327. }
  328. /*****************************************************************************/
  329. static void et61x251_urb_complete(struct urb *urb)
  330. {
  331. struct et61x251_device* cam = urb->context;
  332. struct et61x251_frame_t** f;
  333. size_t imagesize;
  334. u8 i;
  335. int err = 0;
  336. if (urb->status == -ENOENT)
  337. return;
  338. f = &cam->frame_current;
  339. if (cam->stream == STREAM_INTERRUPT) {
  340. cam->stream = STREAM_OFF;
  341. if ((*f))
  342. (*f)->state = F_QUEUED;
  343. DBG(3, "Stream interrupted");
  344. wake_up(&cam->wait_stream);
  345. }
  346. if (cam->state & DEV_DISCONNECTED)
  347. return;
  348. if (cam->state & DEV_MISCONFIGURED) {
  349. wake_up_interruptible(&cam->wait_frame);
  350. return;
  351. }
  352. if (cam->stream == STREAM_OFF || list_empty(&cam->inqueue))
  353. goto resubmit_urb;
  354. if (!(*f))
  355. (*f) = list_entry(cam->inqueue.next, struct et61x251_frame_t,
  356. frame);
  357. imagesize = (cam->sensor.pix_format.width *
  358. cam->sensor.pix_format.height *
  359. cam->sensor.pix_format.priv) / 8;
  360. for (i = 0; i < urb->number_of_packets; i++) {
  361. unsigned int len, status;
  362. void *pos;
  363. u8* b1, * b2, sof;
  364. const u8 VOID_BYTES = 6;
  365. size_t imglen;
  366. len = urb->iso_frame_desc[i].actual_length;
  367. status = urb->iso_frame_desc[i].status;
  368. pos = urb->iso_frame_desc[i].offset + urb->transfer_buffer;
  369. if (status) {
  370. DBG(3, "Error in isochronous frame");
  371. (*f)->state = F_ERROR;
  372. continue;
  373. }
  374. b1 = pos++;
  375. b2 = pos++;
  376. sof = ((*b1 & 0x3f) == 63);
  377. imglen = ((*b1 & 0xc0) << 2) | *b2;
  378. PDBGG("Isochrnous frame: length %u, #%u i, image length %zu",
  379. len, i, imglen);
  380. if ((*f)->state == F_QUEUED || (*f)->state == F_ERROR)
  381. start_of_frame:
  382. if (sof) {
  383. (*f)->state = F_GRABBING;
  384. (*f)->buf.bytesused = 0;
  385. do_gettimeofday(&(*f)->buf.timestamp);
  386. pos += 22;
  387. DBG(3, "SOF detected: new video frame");
  388. }
  389. if ((*f)->state == F_GRABBING) {
  390. if (sof && (*f)->buf.bytesused) {
  391. if (cam->sensor.pix_format.pixelformat ==
  392. V4L2_PIX_FMT_ET61X251)
  393. goto end_of_frame;
  394. else {
  395. DBG(3, "Not expected SOF detected "
  396. "after %lu bytes",
  397. (unsigned long)(*f)->buf.bytesused);
  398. (*f)->state = F_ERROR;
  399. continue;
  400. }
  401. }
  402. if ((*f)->buf.bytesused + imglen > imagesize) {
  403. DBG(3, "Video frame size exceeded");
  404. (*f)->state = F_ERROR;
  405. continue;
  406. }
  407. pos += VOID_BYTES;
  408. memcpy((*f)->bufmem+(*f)->buf.bytesused, pos, imglen);
  409. (*f)->buf.bytesused += imglen;
  410. if ((*f)->buf.bytesused == imagesize) {
  411. u32 b;
  412. end_of_frame:
  413. b = (*f)->buf.bytesused;
  414. (*f)->state = F_DONE;
  415. (*f)->buf.sequence= ++cam->frame_count;
  416. spin_lock(&cam->queue_lock);
  417. list_move_tail(&(*f)->frame, &cam->outqueue);
  418. if (!list_empty(&cam->inqueue))
  419. (*f) = list_entry(cam->inqueue.next,
  420. struct et61x251_frame_t,
  421. frame);
  422. else
  423. (*f) = NULL;
  424. spin_unlock(&cam->queue_lock);
  425. DBG(3, "Video frame captured: : %lu bytes",
  426. (unsigned long)(b));
  427. if (!(*f))
  428. goto resubmit_urb;
  429. if (sof &&
  430. cam->sensor.pix_format.pixelformat ==
  431. V4L2_PIX_FMT_ET61X251)
  432. goto start_of_frame;
  433. }
  434. }
  435. }
  436. resubmit_urb:
  437. urb->dev = cam->usbdev;
  438. err = usb_submit_urb(urb, GFP_ATOMIC);
  439. if (err < 0 && err != -EPERM) {
  440. cam->state |= DEV_MISCONFIGURED;
  441. DBG(1, "usb_submit_urb() failed");
  442. }
  443. wake_up_interruptible(&cam->wait_frame);
  444. }
  445. static int et61x251_start_transfer(struct et61x251_device* cam)
  446. {
  447. struct usb_device *udev = cam->usbdev;
  448. struct urb* urb;
  449. struct usb_host_interface* altsetting = usb_altnum_to_altsetting(
  450. usb_ifnum_to_if(udev, 0),
  451. ET61X251_ALTERNATE_SETTING);
  452. const unsigned int psz = le16_to_cpu(altsetting->
  453. endpoint[0].desc.wMaxPacketSize);
  454. s8 i, j;
  455. int err = 0;
  456. for (i = 0; i < ET61X251_URBS; i++) {
  457. cam->transfer_buffer[i] = kzalloc(ET61X251_ISO_PACKETS * psz,
  458. GFP_KERNEL);
  459. if (!cam->transfer_buffer[i]) {
  460. err = -ENOMEM;
  461. DBG(1, "Not enough memory");
  462. goto free_buffers;
  463. }
  464. }
  465. for (i = 0; i < ET61X251_URBS; i++) {
  466. urb = usb_alloc_urb(ET61X251_ISO_PACKETS, GFP_KERNEL);
  467. cam->urb[i] = urb;
  468. if (!urb) {
  469. err = -ENOMEM;
  470. DBG(1, "usb_alloc_urb() failed");
  471. goto free_urbs;
  472. }
  473. urb->dev = udev;
  474. urb->context = cam;
  475. urb->pipe = usb_rcvisocpipe(udev, 1);
  476. urb->transfer_flags = URB_ISO_ASAP;
  477. urb->number_of_packets = ET61X251_ISO_PACKETS;
  478. urb->complete = et61x251_urb_complete;
  479. urb->transfer_buffer = cam->transfer_buffer[i];
  480. urb->transfer_buffer_length = psz * ET61X251_ISO_PACKETS;
  481. urb->interval = 1;
  482. for (j = 0; j < ET61X251_ISO_PACKETS; j++) {
  483. urb->iso_frame_desc[j].offset = psz * j;
  484. urb->iso_frame_desc[j].length = psz;
  485. }
  486. }
  487. err = et61x251_write_reg(cam, 0x01, 0x03);
  488. err = et61x251_write_reg(cam, 0x00, 0x03);
  489. err = et61x251_write_reg(cam, 0x08, 0x03);
  490. if (err) {
  491. err = -EIO;
  492. DBG(1, "I/O hardware error");
  493. goto free_urbs;
  494. }
  495. err = usb_set_interface(udev, 0, ET61X251_ALTERNATE_SETTING);
  496. if (err) {
  497. DBG(1, "usb_set_interface() failed");
  498. goto free_urbs;
  499. }
  500. cam->frame_current = NULL;
  501. for (i = 0; i < ET61X251_URBS; i++) {
  502. err = usb_submit_urb(cam->urb[i], GFP_KERNEL);
  503. if (err) {
  504. for (j = i-1; j >= 0; j--)
  505. usb_kill_urb(cam->urb[j]);
  506. DBG(1, "usb_submit_urb() failed, error %d", err);
  507. goto free_urbs;
  508. }
  509. }
  510. return 0;
  511. free_urbs:
  512. for (i = 0; (i < ET61X251_URBS) && cam->urb[i]; i++)
  513. usb_free_urb(cam->urb[i]);
  514. free_buffers:
  515. for (i = 0; (i < ET61X251_URBS) && cam->transfer_buffer[i]; i++)
  516. kfree(cam->transfer_buffer[i]);
  517. return err;
  518. }
  519. static int et61x251_stop_transfer(struct et61x251_device* cam)
  520. {
  521. struct usb_device *udev = cam->usbdev;
  522. s8 i;
  523. int err = 0;
  524. if (cam->state & DEV_DISCONNECTED)
  525. return 0;
  526. for (i = ET61X251_URBS-1; i >= 0; i--) {
  527. usb_kill_urb(cam->urb[i]);
  528. usb_free_urb(cam->urb[i]);
  529. kfree(cam->transfer_buffer[i]);
  530. }
  531. err = usb_set_interface(udev, 0, 0); /* 0 Mb/s */
  532. if (err)
  533. DBG(3, "usb_set_interface() failed");
  534. return err;
  535. }
  536. static int et61x251_stream_interrupt(struct et61x251_device* cam)
  537. {
  538. long timeout;
  539. cam->stream = STREAM_INTERRUPT;
  540. timeout = wait_event_timeout(cam->wait_stream,
  541. (cam->stream == STREAM_OFF) ||
  542. (cam->state & DEV_DISCONNECTED),
  543. ET61X251_URB_TIMEOUT);
  544. if (cam->state & DEV_DISCONNECTED)
  545. return -ENODEV;
  546. else if (cam->stream != STREAM_OFF) {
  547. cam->state |= DEV_MISCONFIGURED;
  548. DBG(1, "URB timeout reached. The camera is misconfigured. To "
  549. "use it, close and open /dev/video%d again.",
  550. cam->v4ldev->minor);
  551. return -EIO;
  552. }
  553. return 0;
  554. }
  555. /*****************************************************************************/
  556. #ifdef CONFIG_VIDEO_ADV_DEBUG
  557. static u8 et61x251_strtou8(const char* buff, size_t len, ssize_t* count)
  558. {
  559. char str[5];
  560. char* endp;
  561. unsigned long val;
  562. if (len < 4) {
  563. strncpy(str, buff, len);
  564. str[len] = '\0';
  565. } else {
  566. strncpy(str, buff, 4);
  567. str[4] = '\0';
  568. }
  569. val = simple_strtoul(str, &endp, 0);
  570. *count = 0;
  571. if (val <= 0xff)
  572. *count = (ssize_t)(endp - str);
  573. if ((*count) && (len == *count+1) && (buff[*count] == '\n'))
  574. *count += 1;
  575. return (u8)val;
  576. }
  577. /*
  578. NOTE 1: being inside one of the following methods implies that the v4l
  579. device exists for sure (see kobjects and reference counters)
  580. NOTE 2: buffers are PAGE_SIZE long
  581. */
  582. static ssize_t et61x251_show_reg(struct class_device* cd, char* buf)
  583. {
  584. struct et61x251_device* cam;
  585. ssize_t count;
  586. if (mutex_lock_interruptible(&et61x251_sysfs_lock))
  587. return -ERESTARTSYS;
  588. cam = video_get_drvdata(to_video_device(cd));
  589. if (!cam) {
  590. mutex_unlock(&et61x251_sysfs_lock);
  591. return -ENODEV;
  592. }
  593. count = sprintf(buf, "%u\n", cam->sysfs.reg);
  594. mutex_unlock(&et61x251_sysfs_lock);
  595. return count;
  596. }
  597. static ssize_t
  598. et61x251_store_reg(struct class_device* cd, const char* buf, size_t len)
  599. {
  600. struct et61x251_device* cam;
  601. u8 index;
  602. ssize_t count;
  603. if (mutex_lock_interruptible(&et61x251_sysfs_lock))
  604. return -ERESTARTSYS;
  605. cam = video_get_drvdata(to_video_device(cd));
  606. if (!cam) {
  607. mutex_unlock(&et61x251_sysfs_lock);
  608. return -ENODEV;
  609. }
  610. index = et61x251_strtou8(buf, len, &count);
  611. if (index > 0x8e || !count) {
  612. mutex_unlock(&et61x251_sysfs_lock);
  613. return -EINVAL;
  614. }
  615. cam->sysfs.reg = index;
  616. DBG(2, "Moved ET61X[12]51 register index to 0x%02X", cam->sysfs.reg);
  617. DBG(3, "Written bytes: %zd", count);
  618. mutex_unlock(&et61x251_sysfs_lock);
  619. return count;
  620. }
  621. static ssize_t et61x251_show_val(struct class_device* cd, char* buf)
  622. {
  623. struct et61x251_device* cam;
  624. ssize_t count;
  625. int val;
  626. if (mutex_lock_interruptible(&et61x251_sysfs_lock))
  627. return -ERESTARTSYS;
  628. cam = video_get_drvdata(to_video_device(cd));
  629. if (!cam) {
  630. mutex_unlock(&et61x251_sysfs_lock);
  631. return -ENODEV;
  632. }
  633. if ((val = et61x251_read_reg(cam, cam->sysfs.reg)) < 0) {
  634. mutex_unlock(&et61x251_sysfs_lock);
  635. return -EIO;
  636. }
  637. count = sprintf(buf, "%d\n", val);
  638. DBG(3, "Read bytes: %zd", count);
  639. mutex_unlock(&et61x251_sysfs_lock);
  640. return count;
  641. }
  642. static ssize_t
  643. et61x251_store_val(struct class_device* cd, const char* buf, size_t len)
  644. {
  645. struct et61x251_device* cam;
  646. u8 value;
  647. ssize_t count;
  648. int err;
  649. if (mutex_lock_interruptible(&et61x251_sysfs_lock))
  650. return -ERESTARTSYS;
  651. cam = video_get_drvdata(to_video_device(cd));
  652. if (!cam) {
  653. mutex_unlock(&et61x251_sysfs_lock);
  654. return -ENODEV;
  655. }
  656. value = et61x251_strtou8(buf, len, &count);
  657. if (!count) {
  658. mutex_unlock(&et61x251_sysfs_lock);
  659. return -EINVAL;
  660. }
  661. err = et61x251_write_reg(cam, value, cam->sysfs.reg);
  662. if (err) {
  663. mutex_unlock(&et61x251_sysfs_lock);
  664. return -EIO;
  665. }
  666. DBG(2, "Written ET61X[12]51 reg. 0x%02X, val. 0x%02X",
  667. cam->sysfs.reg, value);
  668. DBG(3, "Written bytes: %zd", count);
  669. mutex_unlock(&et61x251_sysfs_lock);
  670. return count;
  671. }
  672. static ssize_t et61x251_show_i2c_reg(struct class_device* cd, char* buf)
  673. {
  674. struct et61x251_device* cam;
  675. ssize_t count;
  676. if (mutex_lock_interruptible(&et61x251_sysfs_lock))
  677. return -ERESTARTSYS;
  678. cam = video_get_drvdata(to_video_device(cd));
  679. if (!cam) {
  680. mutex_unlock(&et61x251_sysfs_lock);
  681. return -ENODEV;
  682. }
  683. count = sprintf(buf, "%u\n", cam->sysfs.i2c_reg);
  684. DBG(3, "Read bytes: %zd", count);
  685. mutex_unlock(&et61x251_sysfs_lock);
  686. return count;
  687. }
  688. static ssize_t
  689. et61x251_store_i2c_reg(struct class_device* cd, const char* buf, size_t len)
  690. {
  691. struct et61x251_device* cam;
  692. u8 index;
  693. ssize_t count;
  694. if (mutex_lock_interruptible(&et61x251_sysfs_lock))
  695. return -ERESTARTSYS;
  696. cam = video_get_drvdata(to_video_device(cd));
  697. if (!cam) {
  698. mutex_unlock(&et61x251_sysfs_lock);
  699. return -ENODEV;
  700. }
  701. index = et61x251_strtou8(buf, len, &count);
  702. if (!count) {
  703. mutex_unlock(&et61x251_sysfs_lock);
  704. return -EINVAL;
  705. }
  706. cam->sysfs.i2c_reg = index;
  707. DBG(2, "Moved sensor register index to 0x%02X", cam->sysfs.i2c_reg);
  708. DBG(3, "Written bytes: %zd", count);
  709. mutex_unlock(&et61x251_sysfs_lock);
  710. return count;
  711. }
  712. static ssize_t et61x251_show_i2c_val(struct class_device* cd, char* buf)
  713. {
  714. struct et61x251_device* cam;
  715. ssize_t count;
  716. int val;
  717. if (mutex_lock_interruptible(&et61x251_sysfs_lock))
  718. return -ERESTARTSYS;
  719. cam = video_get_drvdata(to_video_device(cd));
  720. if (!cam) {
  721. mutex_unlock(&et61x251_sysfs_lock);
  722. return -ENODEV;
  723. }
  724. if (!(cam->sensor.sysfs_ops & ET61X251_I2C_READ)) {
  725. mutex_unlock(&et61x251_sysfs_lock);
  726. return -ENOSYS;
  727. }
  728. if ((val = et61x251_i2c_read(cam, cam->sysfs.i2c_reg)) < 0) {
  729. mutex_unlock(&et61x251_sysfs_lock);
  730. return -EIO;
  731. }
  732. count = sprintf(buf, "%d\n", val);
  733. DBG(3, "Read bytes: %zd", count);
  734. mutex_unlock(&et61x251_sysfs_lock);
  735. return count;
  736. }
  737. static ssize_t
  738. et61x251_store_i2c_val(struct class_device* cd, const char* buf, size_t len)
  739. {
  740. struct et61x251_device* cam;
  741. u8 value;
  742. ssize_t count;
  743. int err;
  744. if (mutex_lock_interruptible(&et61x251_sysfs_lock))
  745. return -ERESTARTSYS;
  746. cam = video_get_drvdata(to_video_device(cd));
  747. if (!cam) {
  748. mutex_unlock(&et61x251_sysfs_lock);
  749. return -ENODEV;
  750. }
  751. if (!(cam->sensor.sysfs_ops & ET61X251_I2C_READ)) {
  752. mutex_unlock(&et61x251_sysfs_lock);
  753. return -ENOSYS;
  754. }
  755. value = et61x251_strtou8(buf, len, &count);
  756. if (!count) {
  757. mutex_unlock(&et61x251_sysfs_lock);
  758. return -EINVAL;
  759. }
  760. err = et61x251_i2c_write(cam, cam->sysfs.i2c_reg, value);
  761. if (err) {
  762. mutex_unlock(&et61x251_sysfs_lock);
  763. return -EIO;
  764. }
  765. DBG(2, "Written sensor reg. 0x%02X, val. 0x%02X",
  766. cam->sysfs.i2c_reg, value);
  767. DBG(3, "Written bytes: %zd", count);
  768. mutex_unlock(&et61x251_sysfs_lock);
  769. return count;
  770. }
  771. static CLASS_DEVICE_ATTR(reg, S_IRUGO | S_IWUSR,
  772. et61x251_show_reg, et61x251_store_reg);
  773. static CLASS_DEVICE_ATTR(val, S_IRUGO | S_IWUSR,
  774. et61x251_show_val, et61x251_store_val);
  775. static CLASS_DEVICE_ATTR(i2c_reg, S_IRUGO | S_IWUSR,
  776. et61x251_show_i2c_reg, et61x251_store_i2c_reg);
  777. static CLASS_DEVICE_ATTR(i2c_val, S_IRUGO | S_IWUSR,
  778. et61x251_show_i2c_val, et61x251_store_i2c_val);
  779. static int et61x251_create_sysfs(struct et61x251_device* cam)
  780. {
  781. struct class_device *classdev = &(cam->v4ldev->class_dev);
  782. int err = 0;
  783. if ((err = class_device_create_file(classdev, &class_device_attr_reg)))
  784. goto err_out;
  785. if ((err = class_device_create_file(classdev, &class_device_attr_val)))
  786. goto err_reg;
  787. if (cam->sensor.sysfs_ops) {
  788. if ((err = class_device_create_file(classdev,
  789. &class_device_attr_i2c_reg)))
  790. goto err_val;
  791. if ((err = class_device_create_file(classdev,
  792. &class_device_attr_i2c_val)))
  793. goto err_i2c_reg;
  794. }
  795. err_i2c_reg:
  796. if (cam->sensor.sysfs_ops)
  797. class_device_remove_file(classdev, &class_device_attr_i2c_reg);
  798. err_val:
  799. class_device_remove_file(classdev, &class_device_attr_val);
  800. err_reg:
  801. class_device_remove_file(classdev, &class_device_attr_reg);
  802. err_out:
  803. return err;
  804. }
  805. #endif /* CONFIG_VIDEO_ADV_DEBUG */
  806. /*****************************************************************************/
  807. static int
  808. et61x251_set_pix_format(struct et61x251_device* cam,
  809. struct v4l2_pix_format* pix)
  810. {
  811. int r, err = 0;
  812. if ((r = et61x251_read_reg(cam, 0x12)) < 0)
  813. err += r;
  814. if (pix->pixelformat == V4L2_PIX_FMT_ET61X251)
  815. err += et61x251_write_reg(cam, r & 0xfd, 0x12);
  816. else
  817. err += et61x251_write_reg(cam, r | 0x02, 0x12);
  818. return err ? -EIO : 0;
  819. }
  820. static int
  821. et61x251_set_compression(struct et61x251_device* cam,
  822. struct v4l2_jpegcompression* compression)
  823. {
  824. int r, err = 0;
  825. if ((r = et61x251_read_reg(cam, 0x12)) < 0)
  826. err += r;
  827. if (compression->quality == 0)
  828. err += et61x251_write_reg(cam, r & 0xfb, 0x12);
  829. else
  830. err += et61x251_write_reg(cam, r | 0x04, 0x12);
  831. return err ? -EIO : 0;
  832. }
  833. static int et61x251_set_scale(struct et61x251_device* cam, u8 scale)
  834. {
  835. int r = 0, err = 0;
  836. r = et61x251_read_reg(cam, 0x12);
  837. if (r < 0)
  838. err += r;
  839. if (scale == 1)
  840. err += et61x251_write_reg(cam, r & ~0x01, 0x12);
  841. else if (scale == 2)
  842. err += et61x251_write_reg(cam, r | 0x01, 0x12);
  843. if (err)
  844. return -EIO;
  845. PDBGG("Scaling factor: %u", scale);
  846. return 0;
  847. }
  848. static int
  849. et61x251_set_crop(struct et61x251_device* cam, struct v4l2_rect* rect)
  850. {
  851. struct et61x251_sensor* s = &cam->sensor;
  852. u16 fmw_sx = (u16)(rect->left - s->cropcap.bounds.left +
  853. s->active_pixel.left),
  854. fmw_sy = (u16)(rect->top - s->cropcap.bounds.top +
  855. s->active_pixel.top),
  856. fmw_length = (u16)(rect->width),
  857. fmw_height = (u16)(rect->height);
  858. int err = 0;
  859. err += et61x251_write_reg(cam, fmw_sx & 0xff, 0x69);
  860. err += et61x251_write_reg(cam, fmw_sy & 0xff, 0x6a);
  861. err += et61x251_write_reg(cam, fmw_length & 0xff, 0x6b);
  862. err += et61x251_write_reg(cam, fmw_height & 0xff, 0x6c);
  863. err += et61x251_write_reg(cam, (fmw_sx >> 8) | ((fmw_sy & 0x300) >> 6)
  864. | ((fmw_length & 0x300) >> 4)
  865. | ((fmw_height & 0x300) >> 2), 0x6d);
  866. if (err)
  867. return -EIO;
  868. PDBGG("fmw_sx, fmw_sy, fmw_length, fmw_height: %u %u %u %u",
  869. fmw_sx, fmw_sy, fmw_length, fmw_height);
  870. return 0;
  871. }
  872. static int et61x251_init(struct et61x251_device* cam)
  873. {
  874. struct et61x251_sensor* s = &cam->sensor;
  875. struct v4l2_control ctrl;
  876. struct v4l2_queryctrl *qctrl;
  877. struct v4l2_rect* rect;
  878. u8 i = 0;
  879. int err = 0;
  880. if (!(cam->state & DEV_INITIALIZED)) {
  881. mutex_init(&cam->open_mutex);
  882. init_waitqueue_head(&cam->wait_open);
  883. qctrl = s->qctrl;
  884. rect = &(s->cropcap.defrect);
  885. cam->compression.quality = ET61X251_COMPRESSION_QUALITY;
  886. } else { /* use current values */
  887. qctrl = s->_qctrl;
  888. rect = &(s->_rect);
  889. }
  890. err += et61x251_set_scale(cam, rect->width / s->pix_format.width);
  891. err += et61x251_set_crop(cam, rect);
  892. if (err)
  893. return err;
  894. if (s->init) {
  895. err = s->init(cam);
  896. if (err) {
  897. DBG(3, "Sensor initialization failed");
  898. return err;
  899. }
  900. }
  901. err += et61x251_set_compression(cam, &cam->compression);
  902. err += et61x251_set_pix_format(cam, &s->pix_format);
  903. if (s->set_pix_format)
  904. err += s->set_pix_format(cam, &s->pix_format);
  905. if (err)
  906. return err;
  907. if (s->pix_format.pixelformat == V4L2_PIX_FMT_ET61X251)
  908. DBG(3, "Compressed video format is active, quality %d",
  909. cam->compression.quality);
  910. else
  911. DBG(3, "Uncompressed video format is active");
  912. if (s->set_crop)
  913. if ((err = s->set_crop(cam, rect))) {
  914. DBG(3, "set_crop() failed");
  915. return err;
  916. }
  917. if (s->set_ctrl) {
  918. for (i = 0; i < ARRAY_SIZE(s->qctrl); i++)
  919. if (s->qctrl[i].id != 0 &&
  920. !(s->qctrl[i].flags & V4L2_CTRL_FLAG_DISABLED)) {
  921. ctrl.id = s->qctrl[i].id;
  922. ctrl.value = qctrl[i].default_value;
  923. err = s->set_ctrl(cam, &ctrl);
  924. if (err) {
  925. DBG(3, "Set %s control failed",
  926. s->qctrl[i].name);
  927. return err;
  928. }
  929. DBG(3, "Image sensor supports '%s' control",
  930. s->qctrl[i].name);
  931. }
  932. }
  933. if (!(cam->state & DEV_INITIALIZED)) {
  934. mutex_init(&cam->fileop_mutex);
  935. spin_lock_init(&cam->queue_lock);
  936. init_waitqueue_head(&cam->wait_frame);
  937. init_waitqueue_head(&cam->wait_stream);
  938. cam->nreadbuffers = 2;
  939. memcpy(s->_qctrl, s->qctrl, sizeof(s->qctrl));
  940. memcpy(&(s->_rect), &(s->cropcap.defrect),
  941. sizeof(struct v4l2_rect));
  942. cam->state |= DEV_INITIALIZED;
  943. }
  944. DBG(2, "Initialization succeeded");
  945. return 0;
  946. }
  947. /*****************************************************************************/
  948. static void et61x251_release_resources(struct kref *kref)
  949. {
  950. struct et61x251_device *cam;
  951. mutex_lock(&et61x251_sysfs_lock);
  952. cam = container_of(kref, struct et61x251_device, kref);
  953. DBG(2, "V4L2 device /dev/video%d deregistered", cam->v4ldev->minor);
  954. video_set_drvdata(cam->v4ldev, NULL);
  955. video_unregister_device(cam->v4ldev);
  956. usb_put_dev(cam->usbdev);
  957. kfree(cam->control_buffer);
  958. kfree(cam);
  959. mutex_unlock(&et61x251_sysfs_lock);
  960. }
  961. static int et61x251_open(struct inode* inode, struct file* filp)
  962. {
  963. struct et61x251_device* cam;
  964. int err = 0;
  965. if (!down_read_trylock(&et61x251_dev_lock))
  966. return -ERESTARTSYS;
  967. cam = video_get_drvdata(video_devdata(filp));
  968. if (wait_for_completion_interruptible(&cam->probe)) {
  969. up_read(&et61x251_dev_lock);
  970. return -ERESTARTSYS;
  971. }
  972. kref_get(&cam->kref);
  973. if (mutex_lock_interruptible(&cam->open_mutex)) {
  974. kref_put(&cam->kref, et61x251_release_resources);
  975. up_read(&et61x251_dev_lock);
  976. return -ERESTARTSYS;
  977. }
  978. if (cam->state & DEV_DISCONNECTED) {
  979. DBG(1, "Device not present");
  980. err = -ENODEV;
  981. goto out;
  982. }
  983. if (cam->users) {
  984. DBG(2, "Device /dev/video%d is already in use",
  985. cam->v4ldev->minor);
  986. DBG(3, "Simultaneous opens are not supported");
  987. if ((filp->f_flags & O_NONBLOCK) ||
  988. (filp->f_flags & O_NDELAY)) {
  989. err = -EWOULDBLOCK;
  990. goto out;
  991. }
  992. DBG(2, "A blocking open() has been requested. Wait for the "
  993. "device to be released...");
  994. up_read(&et61x251_dev_lock);
  995. err = wait_event_interruptible_exclusive(cam->wait_open,
  996. (cam->state & DEV_DISCONNECTED)
  997. || !cam->users);
  998. down_read(&et61x251_dev_lock);
  999. if (err)
  1000. goto out;
  1001. if (cam->state & DEV_DISCONNECTED) {
  1002. err = -ENODEV;
  1003. goto out;
  1004. }
  1005. }
  1006. if (cam->state & DEV_MISCONFIGURED) {
  1007. err = et61x251_init(cam);
  1008. if (err) {
  1009. DBG(1, "Initialization failed again. "
  1010. "I will retry on next open().");
  1011. goto out;
  1012. }
  1013. cam->state &= ~DEV_MISCONFIGURED;
  1014. }
  1015. if ((err = et61x251_start_transfer(cam)))
  1016. goto out;
  1017. filp->private_data = cam;
  1018. cam->users++;
  1019. cam->io = IO_NONE;
  1020. cam->stream = STREAM_OFF;
  1021. cam->nbuffers = 0;
  1022. cam->frame_count = 0;
  1023. et61x251_empty_framequeues(cam);
  1024. DBG(3, "Video device /dev/video%d is open", cam->v4ldev->minor);
  1025. out:
  1026. mutex_unlock(&cam->open_mutex);
  1027. if (err)
  1028. kref_put(&cam->kref, et61x251_release_resources);
  1029. up_read(&et61x251_dev_lock);
  1030. return err;
  1031. }
  1032. static int et61x251_release(struct inode* inode, struct file* filp)
  1033. {
  1034. struct et61x251_device* cam;
  1035. down_write(&et61x251_dev_lock);
  1036. cam = video_get_drvdata(video_devdata(filp));
  1037. et61x251_stop_transfer(cam);
  1038. et61x251_release_buffers(cam);
  1039. cam->users--;
  1040. wake_up_interruptible_nr(&cam->wait_open, 1);
  1041. DBG(3, "Video device /dev/video%d closed", cam->v4ldev->minor);
  1042. kref_put(&cam->kref, et61x251_release_resources);
  1043. up_write(&et61x251_dev_lock);
  1044. return 0;
  1045. }
  1046. static ssize_t
  1047. et61x251_read(struct file* filp, char __user * buf,
  1048. size_t count, loff_t* f_pos)
  1049. {
  1050. struct et61x251_device* cam = video_get_drvdata(video_devdata(filp));
  1051. struct et61x251_frame_t* f, * i;
  1052. unsigned long lock_flags;
  1053. long timeout;
  1054. int err = 0;
  1055. if (mutex_lock_interruptible(&cam->fileop_mutex))
  1056. return -ERESTARTSYS;
  1057. if (cam->state & DEV_DISCONNECTED) {
  1058. DBG(1, "Device not present");
  1059. mutex_unlock(&cam->fileop_mutex);
  1060. return -ENODEV;
  1061. }
  1062. if (cam->state & DEV_MISCONFIGURED) {
  1063. DBG(1, "The camera is misconfigured. Close and open it "
  1064. "again.");
  1065. mutex_unlock(&cam->fileop_mutex);
  1066. return -EIO;
  1067. }
  1068. if (cam->io == IO_MMAP) {
  1069. DBG(3, "Close and open the device again to choose the read "
  1070. "method");
  1071. mutex_unlock(&cam->fileop_mutex);
  1072. return -EBUSY;
  1073. }
  1074. if (cam->io == IO_NONE) {
  1075. if (!et61x251_request_buffers(cam, cam->nreadbuffers,
  1076. IO_READ)) {
  1077. DBG(1, "read() failed, not enough memory");
  1078. mutex_unlock(&cam->fileop_mutex);
  1079. return -ENOMEM;
  1080. }
  1081. cam->io = IO_READ;
  1082. cam->stream = STREAM_ON;
  1083. }
  1084. if (list_empty(&cam->inqueue)) {
  1085. if (!list_empty(&cam->outqueue))
  1086. et61x251_empty_framequeues(cam);
  1087. et61x251_queue_unusedframes(cam);
  1088. }
  1089. if (!count) {
  1090. mutex_unlock(&cam->fileop_mutex);
  1091. return 0;
  1092. }
  1093. if (list_empty(&cam->outqueue)) {
  1094. if (filp->f_flags & O_NONBLOCK) {
  1095. mutex_unlock(&cam->fileop_mutex);
  1096. return -EAGAIN;
  1097. }
  1098. timeout = wait_event_interruptible_timeout
  1099. ( cam->wait_frame,
  1100. (!list_empty(&cam->outqueue)) ||
  1101. (cam->state & DEV_DISCONNECTED) ||
  1102. (cam->state & DEV_MISCONFIGURED),
  1103. cam->module_param.frame_timeout *
  1104. 1000 * msecs_to_jiffies(1) );
  1105. if (timeout < 0) {
  1106. mutex_unlock(&cam->fileop_mutex);
  1107. return timeout;
  1108. }
  1109. if (cam->state & DEV_DISCONNECTED) {
  1110. mutex_unlock(&cam->fileop_mutex);
  1111. return -ENODEV;
  1112. }
  1113. if (!timeout || (cam->state & DEV_MISCONFIGURED)) {
  1114. mutex_unlock(&cam->fileop_mutex);
  1115. return -EIO;
  1116. }
  1117. }
  1118. f = list_entry(cam->outqueue.prev, struct et61x251_frame_t, frame);
  1119. if (count > f->buf.bytesused)
  1120. count = f->buf.bytesused;
  1121. if (copy_to_user(buf, f->bufmem, count)) {
  1122. err = -EFAULT;
  1123. goto exit;
  1124. }
  1125. *f_pos += count;
  1126. exit:
  1127. spin_lock_irqsave(&cam->queue_lock, lock_flags);
  1128. list_for_each_entry(i, &cam->outqueue, frame)
  1129. i->state = F_UNUSED;
  1130. INIT_LIST_HEAD(&cam->outqueue);
  1131. spin_unlock_irqrestore(&cam->queue_lock, lock_flags);
  1132. et61x251_queue_unusedframes(cam);
  1133. PDBGG("Frame #%lu, bytes read: %zu",
  1134. (unsigned long)f->buf.index, count);
  1135. mutex_unlock(&cam->fileop_mutex);
  1136. return err ? err : count;
  1137. }
  1138. static unsigned int et61x251_poll(struct file *filp, poll_table *wait)
  1139. {
  1140. struct et61x251_device* cam = video_get_drvdata(video_devdata(filp));
  1141. struct et61x251_frame_t* f;
  1142. unsigned long lock_flags;
  1143. unsigned int mask = 0;
  1144. if (mutex_lock_interruptible(&cam->fileop_mutex))
  1145. return POLLERR;
  1146. if (cam->state & DEV_DISCONNECTED) {
  1147. DBG(1, "Device not present");
  1148. goto error;
  1149. }
  1150. if (cam->state & DEV_MISCONFIGURED) {
  1151. DBG(1, "The camera is misconfigured. Close and open it "
  1152. "again.");
  1153. goto error;
  1154. }
  1155. if (cam->io == IO_NONE) {
  1156. if (!et61x251_request_buffers(cam, cam->nreadbuffers,
  1157. IO_READ)) {
  1158. DBG(1, "poll() failed, not enough memory");
  1159. goto error;
  1160. }
  1161. cam->io = IO_READ;
  1162. cam->stream = STREAM_ON;
  1163. }
  1164. if (cam->io == IO_READ) {
  1165. spin_lock_irqsave(&cam->queue_lock, lock_flags);
  1166. list_for_each_entry(f, &cam->outqueue, frame)
  1167. f->state = F_UNUSED;
  1168. INIT_LIST_HEAD(&cam->outqueue);
  1169. spin_unlock_irqrestore(&cam->queue_lock, lock_flags);
  1170. et61x251_queue_unusedframes(cam);
  1171. }
  1172. poll_wait(filp, &cam->wait_frame, wait);
  1173. if (!list_empty(&cam->outqueue))
  1174. mask |= POLLIN | POLLRDNORM;
  1175. mutex_unlock(&cam->fileop_mutex);
  1176. return mask;
  1177. error:
  1178. mutex_unlock(&cam->fileop_mutex);
  1179. return POLLERR;
  1180. }
  1181. static void et61x251_vm_open(struct vm_area_struct* vma)
  1182. {
  1183. struct et61x251_frame_t* f = vma->vm_private_data;
  1184. f->vma_use_count++;
  1185. }
  1186. static void et61x251_vm_close(struct vm_area_struct* vma)
  1187. {
  1188. /* NOTE: buffers are not freed here */
  1189. struct et61x251_frame_t* f = vma->vm_private_data;
  1190. f->vma_use_count--;
  1191. }
  1192. static struct vm_operations_struct et61x251_vm_ops = {
  1193. .open = et61x251_vm_open,
  1194. .close = et61x251_vm_close,
  1195. };
  1196. static int et61x251_mmap(struct file* filp, struct vm_area_struct *vma)
  1197. {
  1198. struct et61x251_device* cam = video_get_drvdata(video_devdata(filp));
  1199. unsigned long size = vma->vm_end - vma->vm_start,
  1200. start = vma->vm_start;
  1201. void *pos;
  1202. u32 i;
  1203. if (mutex_lock_interruptible(&cam->fileop_mutex))
  1204. return -ERESTARTSYS;
  1205. if (cam->state & DEV_DISCONNECTED) {
  1206. DBG(1, "Device not present");
  1207. mutex_unlock(&cam->fileop_mutex);
  1208. return -ENODEV;
  1209. }
  1210. if (cam->state & DEV_MISCONFIGURED) {
  1211. DBG(1, "The camera is misconfigured. Close and open it "
  1212. "again.");
  1213. mutex_unlock(&cam->fileop_mutex);
  1214. return -EIO;
  1215. }
  1216. if (!(vma->vm_flags & (VM_WRITE | VM_READ))) {
  1217. mutex_unlock(&cam->fileop_mutex);
  1218. return -EACCES;
  1219. }
  1220. if (cam->io != IO_MMAP ||
  1221. size != PAGE_ALIGN(cam->frame[0].buf.length)) {
  1222. mutex_unlock(&cam->fileop_mutex);
  1223. return -EINVAL;
  1224. }
  1225. for (i = 0; i < cam->nbuffers; i++) {
  1226. if ((cam->frame[i].buf.m.offset>>PAGE_SHIFT) == vma->vm_pgoff)
  1227. break;
  1228. }
  1229. if (i == cam->nbuffers) {
  1230. mutex_unlock(&cam->fileop_mutex);
  1231. return -EINVAL;
  1232. }
  1233. vma->vm_flags |= VM_IO;
  1234. vma->vm_flags |= VM_RESERVED;
  1235. pos = cam->frame[i].bufmem;
  1236. while (size > 0) { /* size is page-aligned */
  1237. if (vm_insert_page(vma, start, vmalloc_to_page(pos))) {
  1238. mutex_unlock(&cam->fileop_mutex);
  1239. return -EAGAIN;
  1240. }
  1241. start += PAGE_SIZE;
  1242. pos += PAGE_SIZE;
  1243. size -= PAGE_SIZE;
  1244. }
  1245. vma->vm_ops = &et61x251_vm_ops;
  1246. vma->vm_private_data = &cam->frame[i];
  1247. et61x251_vm_open(vma);
  1248. mutex_unlock(&cam->fileop_mutex);
  1249. return 0;
  1250. }
  1251. /*****************************************************************************/
  1252. static int
  1253. et61x251_vidioc_querycap(struct et61x251_device* cam, void __user * arg)
  1254. {
  1255. struct v4l2_capability cap = {
  1256. .driver = "et61x251",
  1257. .version = ET61X251_MODULE_VERSION_CODE,
  1258. .capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_READWRITE |
  1259. V4L2_CAP_STREAMING,
  1260. };
  1261. strlcpy(cap.card, cam->v4ldev->name, sizeof(cap.card));
  1262. if (usb_make_path(cam->usbdev, cap.bus_info, sizeof(cap.bus_info)) < 0)
  1263. strlcpy(cap.bus_info, cam->usbdev->dev.bus_id,
  1264. sizeof(cap.bus_info));
  1265. if (copy_to_user(arg, &cap, sizeof(cap)))
  1266. return -EFAULT;
  1267. return 0;
  1268. }
  1269. static int
  1270. et61x251_vidioc_enuminput(struct et61x251_device* cam, void __user * arg)
  1271. {
  1272. struct v4l2_input i;
  1273. if (copy_from_user(&i, arg, sizeof(i)))
  1274. return -EFAULT;
  1275. if (i.index)
  1276. return -EINVAL;
  1277. memset(&i, 0, sizeof(i));
  1278. strcpy(i.name, "Camera");
  1279. i.type = V4L2_INPUT_TYPE_CAMERA;
  1280. if (copy_to_user(arg, &i, sizeof(i)))
  1281. return -EFAULT;
  1282. return 0;
  1283. }
  1284. static int
  1285. et61x251_vidioc_g_input(struct et61x251_device* cam, void __user * arg)
  1286. {
  1287. int index = 0;
  1288. if (copy_to_user(arg, &index, sizeof(index)))
  1289. return -EFAULT;
  1290. return 0;
  1291. }
  1292. static int
  1293. et61x251_vidioc_s_input(struct et61x251_device* cam, void __user * arg)
  1294. {
  1295. int index;
  1296. if (copy_from_user(&index, arg, sizeof(index)))
  1297. return -EFAULT;
  1298. if (index != 0)
  1299. return -EINVAL;
  1300. return 0;
  1301. }
  1302. static int
  1303. et61x251_vidioc_query_ctrl(struct et61x251_device* cam, void __user * arg)
  1304. {
  1305. struct et61x251_sensor* s = &cam->sensor;
  1306. struct v4l2_queryctrl qc;
  1307. u8 i;
  1308. if (copy_from_user(&qc, arg, sizeof(qc)))
  1309. return -EFAULT;
  1310. for (i = 0; i < ARRAY_SIZE(s->qctrl); i++)
  1311. if (qc.id && qc.id == s->qctrl[i].id) {
  1312. memcpy(&qc, &(s->qctrl[i]), sizeof(qc));
  1313. if (copy_to_user(arg, &qc, sizeof(qc)))
  1314. return -EFAULT;
  1315. return 0;
  1316. }
  1317. return -EINVAL;
  1318. }
  1319. static int
  1320. et61x251_vidioc_g_ctrl(struct et61x251_device* cam, void __user * arg)
  1321. {
  1322. struct et61x251_sensor* s = &cam->sensor;
  1323. struct v4l2_control ctrl;
  1324. int err = 0;
  1325. u8 i;
  1326. if (!s->get_ctrl && !s->set_ctrl)
  1327. return -EINVAL;
  1328. if (copy_from_user(&ctrl, arg, sizeof(ctrl)))
  1329. return -EFAULT;
  1330. if (!s->get_ctrl) {
  1331. for (i = 0; i < ARRAY_SIZE(s->qctrl); i++)
  1332. if (ctrl.id == s->qctrl[i].id) {
  1333. ctrl.value = s->_qctrl[i].default_value;
  1334. goto exit;
  1335. }
  1336. return -EINVAL;
  1337. } else
  1338. err = s->get_ctrl(cam, &ctrl);
  1339. exit:
  1340. if (copy_to_user(arg, &ctrl, sizeof(ctrl)))
  1341. return -EFAULT;
  1342. return err;
  1343. }
  1344. static int
  1345. et61x251_vidioc_s_ctrl(struct et61x251_device* cam, void __user * arg)
  1346. {
  1347. struct et61x251_sensor* s = &cam->sensor;
  1348. struct v4l2_control ctrl;
  1349. u8 i;
  1350. int err = 0;
  1351. if (!s->set_ctrl)
  1352. return -EINVAL;
  1353. if (copy_from_user(&ctrl, arg, sizeof(ctrl)))
  1354. return -EFAULT;
  1355. for (i = 0; i < ARRAY_SIZE(s->qctrl); i++)
  1356. if (ctrl.id == s->qctrl[i].id) {
  1357. if (s->qctrl[i].flags & V4L2_CTRL_FLAG_DISABLED)
  1358. return -EINVAL;
  1359. if (ctrl.value < s->qctrl[i].minimum ||
  1360. ctrl.value > s->qctrl[i].maximum)
  1361. return -ERANGE;
  1362. ctrl.value -= ctrl.value % s->qctrl[i].step;
  1363. break;
  1364. }
  1365. if ((err = s->set_ctrl(cam, &ctrl)))
  1366. return err;
  1367. s->_qctrl[i].default_value = ctrl.value;
  1368. return 0;
  1369. }
  1370. static int
  1371. et61x251_vidioc_cropcap(struct et61x251_device* cam, void __user * arg)
  1372. {
  1373. struct v4l2_cropcap* cc = &(cam->sensor.cropcap);
  1374. cc->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  1375. cc->pixelaspect.numerator = 1;
  1376. cc->pixelaspect.denominator = 1;
  1377. if (copy_to_user(arg, cc, sizeof(*cc)))
  1378. return -EFAULT;
  1379. return 0;
  1380. }
  1381. static int
  1382. et61x251_vidioc_g_crop(struct et61x251_device* cam, void __user * arg)
  1383. {
  1384. struct et61x251_sensor* s = &cam->sensor;
  1385. struct v4l2_crop crop = {
  1386. .type = V4L2_BUF_TYPE_VIDEO_CAPTURE,
  1387. };
  1388. memcpy(&(crop.c), &(s->_rect), sizeof(struct v4l2_rect));
  1389. if (copy_to_user(arg, &crop, sizeof(crop)))
  1390. return -EFAULT;
  1391. return 0;
  1392. }
  1393. static int
  1394. et61x251_vidioc_s_crop(struct et61x251_device* cam, void __user * arg)
  1395. {
  1396. struct et61x251_sensor* s = &cam->sensor;
  1397. struct v4l2_crop crop;
  1398. struct v4l2_rect* rect;
  1399. struct v4l2_rect* bounds = &(s->cropcap.bounds);
  1400. struct v4l2_pix_format* pix_format = &(s->pix_format);
  1401. u8 scale;
  1402. const enum et61x251_stream_state stream = cam->stream;
  1403. const u32 nbuffers = cam->nbuffers;
  1404. u32 i;
  1405. int err = 0;
  1406. if (copy_from_user(&crop, arg, sizeof(crop)))
  1407. return -EFAULT;
  1408. rect = &(crop.c);
  1409. if (crop.type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
  1410. return -EINVAL;
  1411. if (cam->module_param.force_munmap)
  1412. for (i = 0; i < cam->nbuffers; i++)
  1413. if (cam->frame[i].vma_use_count) {
  1414. DBG(3, "VIDIOC_S_CROP failed. "
  1415. "Unmap the buffers first.");
  1416. return -EBUSY;
  1417. }
  1418. /* Preserve R,G or B origin */
  1419. rect->left = (s->_rect.left & 1L) ? rect->left | 1L : rect->left & ~1L;
  1420. rect->top = (s->_rect.top & 1L) ? rect->top | 1L : rect->top & ~1L;
  1421. if (rect->width < 16)
  1422. rect->width = 16;
  1423. if (rect->height < 16)
  1424. rect->height = 16;
  1425. if (rect->width > bounds->width)
  1426. rect->width = bounds->width;
  1427. if (rect->height > bounds->height)
  1428. rect->height = bounds->height;
  1429. if (rect->left < bounds->left)
  1430. rect->left = bounds->left;
  1431. if (rect->top < bounds->top)
  1432. rect->top = bounds->top;
  1433. if (rect->left + rect->width > bounds->left + bounds->width)
  1434. rect->left = bounds->left+bounds->width - rect->width;
  1435. if (rect->top + rect->height > bounds->top + bounds->height)
  1436. rect->top = bounds->top+bounds->height - rect->height;
  1437. rect->width &= ~15L;
  1438. rect->height &= ~15L;
  1439. if (ET61X251_PRESERVE_IMGSCALE) {
  1440. /* Calculate the actual scaling factor */
  1441. u32 a, b;
  1442. a = rect->width * rect->height;
  1443. b = pix_format->width * pix_format->height;
  1444. scale = b ? (u8)((a / b) < 4 ? 1 : 2) : 1;
  1445. } else
  1446. scale = 1;
  1447. if (cam->stream == STREAM_ON)
  1448. if ((err = et61x251_stream_interrupt(cam)))
  1449. return err;
  1450. if (copy_to_user(arg, &crop, sizeof(crop))) {
  1451. cam->stream = stream;
  1452. return -EFAULT;
  1453. }
  1454. if (cam->module_param.force_munmap || cam->io == IO_READ)
  1455. et61x251_release_buffers(cam);
  1456. err = et61x251_set_crop(cam, rect);
  1457. if (s->set_crop)
  1458. err += s->set_crop(cam, rect);
  1459. err += et61x251_set_scale(cam, scale);
  1460. if (err) { /* atomic, no rollback in ioctl() */
  1461. cam->state |= DEV_MISCONFIGURED;
  1462. DBG(1, "VIDIOC_S_CROP failed because of hardware problems. To "
  1463. "use the camera, close and open /dev/video%d again.",
  1464. cam->v4ldev->minor);
  1465. return -EIO;
  1466. }
  1467. s->pix_format.width = rect->width/scale;
  1468. s->pix_format.height = rect->height/scale;
  1469. memcpy(&(s->_rect), rect, sizeof(*rect));
  1470. if ((cam->module_param.force_munmap || cam->io == IO_READ) &&
  1471. nbuffers != et61x251_request_buffers(cam, nbuffers, cam->io)) {
  1472. cam->state |= DEV_MISCONFIGURED;
  1473. DBG(1, "VIDIOC_S_CROP failed because of not enough memory. To "
  1474. "use the camera, close and open /dev/video%d again.",
  1475. cam->v4ldev->minor);
  1476. return -ENOMEM;
  1477. }
  1478. if (cam->io == IO_READ)
  1479. et61x251_empty_framequeues(cam);
  1480. else if (cam->module_param.force_munmap)
  1481. et61x251_requeue_outqueue(cam);
  1482. cam->stream = stream;
  1483. return 0;
  1484. }
  1485. static int
  1486. et61x251_vidioc_enum_framesizes(struct et61x251_device* cam, void __user * arg)
  1487. {
  1488. struct v4l2_frmsizeenum frmsize;
  1489. if (copy_from_user(&frmsize, arg, sizeof(frmsize)))
  1490. return -EFAULT;
  1491. if (frmsize.index != 0)
  1492. return -EINVAL;
  1493. if (frmsize.pixel_format != V4L2_PIX_FMT_ET61X251 &&
  1494. frmsize.pixel_format != V4L2_PIX_FMT_SBGGR8)
  1495. return -EINVAL;
  1496. frmsize.type = V4L2_FRMSIZE_TYPE_STEPWISE;
  1497. frmsize.stepwise.min_width = frmsize.stepwise.step_width = 16;
  1498. frmsize.stepwise.min_height = frmsize.stepwise.step_height = 16;
  1499. frmsize.stepwise.max_width = cam->sensor.cropcap.bounds.width;
  1500. frmsize.stepwise.max_height = cam->sensor.cropcap.bounds.height;
  1501. memset(&frmsize.reserved, 0, sizeof(frmsize.reserved));
  1502. if (copy_to_user(arg, &frmsize, sizeof(frmsize)))
  1503. return -EFAULT;
  1504. return 0;
  1505. }
  1506. static int
  1507. et61x251_vidioc_enum_fmt(struct et61x251_device* cam, void __user * arg)
  1508. {
  1509. struct v4l2_fmtdesc fmtd;
  1510. if (copy_from_user(&fmtd, arg, sizeof(fmtd)))
  1511. return -EFAULT;
  1512. if (fmtd.type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
  1513. return -EINVAL;
  1514. if (fmtd.index == 0) {
  1515. strcpy(fmtd.description, "bayer rgb");
  1516. fmtd.pixelformat = V4L2_PIX_FMT_SBGGR8;
  1517. } else if (fmtd.index == 1) {
  1518. strcpy(fmtd.description, "compressed");
  1519. fmtd.pixelformat = V4L2_PIX_FMT_ET61X251;
  1520. fmtd.flags = V4L2_FMT_FLAG_COMPRESSED;
  1521. } else
  1522. return -EINVAL;
  1523. fmtd.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  1524. memset(&fmtd.reserved, 0, sizeof(fmtd.reserved));
  1525. if (copy_to_user(arg, &fmtd, sizeof(fmtd)))
  1526. return -EFAULT;
  1527. return 0;
  1528. }
  1529. static int
  1530. et61x251_vidioc_g_fmt(struct et61x251_device* cam, void __user * arg)
  1531. {
  1532. struct v4l2_format format;
  1533. struct v4l2_pix_format* pfmt = &(cam->sensor.pix_format);
  1534. if (copy_from_user(&format, arg, sizeof(format)))
  1535. return -EFAULT;
  1536. if (format.type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
  1537. return -EINVAL;
  1538. pfmt->colorspace = (pfmt->pixelformat == V4L2_PIX_FMT_ET61X251) ?
  1539. 0 : V4L2_COLORSPACE_SRGB;
  1540. pfmt->bytesperline = (pfmt->pixelformat==V4L2_PIX_FMT_ET61X251)
  1541. ? 0 : (pfmt->width * pfmt->priv) / 8;
  1542. pfmt->sizeimage = pfmt->height * ((pfmt->width*pfmt->priv)/8);
  1543. pfmt->field = V4L2_FIELD_NONE;
  1544. memcpy(&(format.fmt.pix), pfmt, sizeof(*pfmt));
  1545. if (copy_to_user(arg, &format, sizeof(format)))
  1546. return -EFAULT;
  1547. return 0;
  1548. }
  1549. static int
  1550. et61x251_vidioc_try_s_fmt(struct et61x251_device* cam, unsigned int cmd,
  1551. void __user * arg)
  1552. {
  1553. struct et61x251_sensor* s = &cam->sensor;
  1554. struct v4l2_format format;
  1555. struct v4l2_pix_format* pix;
  1556. struct v4l2_pix_format* pfmt = &(s->pix_format);
  1557. struct v4l2_rect* bounds = &(s->cropcap.bounds);
  1558. struct v4l2_rect rect;
  1559. u8 scale;
  1560. const enum et61x251_stream_state stream = cam->stream;
  1561. const u32 nbuffers = cam->nbuffers;
  1562. u32 i;
  1563. int err = 0;
  1564. if (copy_from_user(&format, arg, sizeof(format)))
  1565. return -EFAULT;
  1566. pix = &(format.fmt.pix);
  1567. if (format.type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
  1568. return -EINVAL;
  1569. memcpy(&rect, &(s->_rect), sizeof(rect));
  1570. { /* calculate the actual scaling factor */
  1571. u32 a, b;
  1572. a = rect.width * rect.height;
  1573. b = pix->width * pix->height;
  1574. scale = b ? (u8)((a / b) < 4 ? 1 : 2) : 1;
  1575. }
  1576. rect.width = scale * pix->width;
  1577. rect.height = scale * pix->height;
  1578. if (rect.width < 16)
  1579. rect.width = 16;
  1580. if (rect.height < 16)
  1581. rect.height = 16;
  1582. if (rect.width > bounds->left + bounds->width - rect.left)
  1583. rect.width = bounds->left + bounds->width - rect.left;
  1584. if (rect.height > bounds->top + bounds->height - rect.top)
  1585. rect.height = bounds->top + bounds->height - rect.top;
  1586. rect.width &= ~15L;
  1587. rect.height &= ~15L;
  1588. { /* adjust the scaling factor */
  1589. u32 a, b;
  1590. a = rect.width * rect.height;
  1591. b = pix->width * pix->height;
  1592. scale = b ? (u8)((a / b) < 4 ? 1 : 2) : 1;
  1593. }
  1594. pix->width = rect.width / scale;
  1595. pix->height = rect.height / scale;
  1596. if (pix->pixelformat != V4L2_PIX_FMT_ET61X251 &&
  1597. pix->pixelformat != V4L2_PIX_FMT_SBGGR8)
  1598. pix->pixelformat = pfmt->pixelformat;
  1599. pix->priv = pfmt->priv; /* bpp */
  1600. pix->colorspace = (pix->pixelformat == V4L2_PIX_FMT_ET61X251) ?
  1601. 0 : V4L2_COLORSPACE_SRGB;
  1602. pix->colorspace = pfmt->colorspace;
  1603. pix->bytesperline = (pix->pixelformat == V4L2_PIX_FMT_ET61X251)
  1604. ? 0 : (pix->width * pix->priv) / 8;
  1605. pix->sizeimage = pix->height * ((pix->width * pix->priv) / 8);
  1606. pix->field = V4L2_FIELD_NONE;
  1607. if (cmd == VIDIOC_TRY_FMT) {
  1608. if (copy_to_user(arg, &format, sizeof(format)))
  1609. return -EFAULT;
  1610. return 0;
  1611. }
  1612. if (cam->module_param.force_munmap)
  1613. for (i = 0; i < cam->nbuffers; i++)
  1614. if (cam->frame[i].vma_use_count) {
  1615. DBG(3, "VIDIOC_S_FMT failed. "
  1616. "Unmap the buffers first.");
  1617. return -EBUSY;
  1618. }
  1619. if (cam->stream == STREAM_ON)
  1620. if ((err = et61x251_stream_interrupt(cam)))
  1621. return err;
  1622. if (copy_to_user(arg, &format, sizeof(format))) {
  1623. cam->stream = stream;
  1624. return -EFAULT;
  1625. }
  1626. if (cam->module_param.force_munmap || cam->io == IO_READ)
  1627. et61x251_release_buffers(cam);
  1628. err += et61x251_set_pix_format(cam, pix);
  1629. err += et61x251_set_crop(cam, &rect);
  1630. if (s->set_pix_format)
  1631. err += s->set_pix_format(cam, pix);
  1632. if (s->set_crop)
  1633. err += s->set_crop(cam, &rect);
  1634. err += et61x251_set_scale(cam, scale);
  1635. if (err) { /* atomic, no rollback in ioctl() */
  1636. cam->state |= DEV_MISCONFIGURED;
  1637. DBG(1, "VIDIOC_S_FMT failed because of hardware problems. To "
  1638. "use the camera, close and open /dev/video%d again.",
  1639. cam->v4ldev->minor);
  1640. return -EIO;
  1641. }
  1642. memcpy(pfmt, pix, sizeof(*pix));
  1643. memcpy(&(s->_rect), &rect, sizeof(rect));
  1644. if ((cam->module_param.force_munmap || cam->io == IO_READ) &&
  1645. nbuffers != et61x251_request_buffers(cam, nbuffers, cam->io)) {
  1646. cam->state |= DEV_MISCONFIGURED;
  1647. DBG(1, "VIDIOC_S_FMT failed because of not enough memory. To "
  1648. "use the camera, close and open /dev/video%d again.",
  1649. cam->v4ldev->minor);
  1650. return -ENOMEM;
  1651. }
  1652. if (cam->io == IO_READ)
  1653. et61x251_empty_framequeues(cam);
  1654. else if (cam->module_param.force_munmap)
  1655. et61x251_requeue_outqueue(cam);
  1656. cam->stream = stream;
  1657. return 0;
  1658. }
  1659. static int
  1660. et61x251_vidioc_g_jpegcomp(struct et61x251_device* cam, void __user * arg)
  1661. {
  1662. if (copy_to_user(arg, &cam->compression,
  1663. sizeof(cam->compression)))
  1664. return -EFAULT;
  1665. return 0;
  1666. }
  1667. static int
  1668. et61x251_vidioc_s_jpegcomp(struct et61x251_device* cam, void __user * arg)
  1669. {
  1670. struct v4l2_jpegcompression jc;
  1671. const enum et61x251_stream_state stream = cam->stream;
  1672. int err = 0;
  1673. if (copy_from_user(&jc, arg, sizeof(jc)))
  1674. return -EFAULT;
  1675. if (jc.quality != 0 && jc.quality != 1)
  1676. return -EINVAL;
  1677. if (cam->stream == STREAM_ON)
  1678. if ((err = et61x251_stream_interrupt(cam)))
  1679. return err;
  1680. err += et61x251_set_compression(cam, &jc);
  1681. if (err) { /* atomic, no rollback in ioctl() */
  1682. cam->state |= DEV_MISCONFIGURED;
  1683. DBG(1, "VIDIOC_S_JPEGCOMP failed because of hardware "
  1684. "problems. To use the camera, close and open "
  1685. "/dev/video%d again.", cam->v4ldev->minor);
  1686. return -EIO;
  1687. }
  1688. cam->compression.quality = jc.quality;
  1689. cam->stream = stream;
  1690. return 0;
  1691. }
  1692. static int
  1693. et61x251_vidioc_reqbufs(struct et61x251_device* cam, void __user * arg)
  1694. {
  1695. struct v4l2_requestbuffers rb;
  1696. u32 i;
  1697. int err;
  1698. if (copy_from_user(&rb, arg, sizeof(rb)))
  1699. return -EFAULT;
  1700. if (rb.type != V4L2_BUF_TYPE_VIDEO_CAPTURE ||
  1701. rb.memory != V4L2_MEMORY_MMAP)
  1702. return -EINVAL;
  1703. if (cam->io == IO_READ) {
  1704. DBG(3, "Close and open the device again to choose the mmap "
  1705. "I/O method");
  1706. return -EBUSY;
  1707. }
  1708. for (i = 0; i < cam->nbuffers; i++)
  1709. if (cam->frame[i].vma_use_count) {
  1710. DBG(3, "VIDIOC_REQBUFS failed. "
  1711. "Previous buffers are still mapped.");
  1712. return -EBUSY;
  1713. }
  1714. if (cam->stream == STREAM_ON)
  1715. if ((err = et61x251_stream_interrupt(cam)))
  1716. return err;
  1717. et61x251_empty_framequeues(cam);
  1718. et61x251_release_buffers(cam);
  1719. if (rb.count)
  1720. rb.count = et61x251_request_buffers(cam, rb.count, IO_MMAP);
  1721. if (copy_to_user(arg, &rb, sizeof(rb))) {
  1722. et61x251_release_buffers(cam);
  1723. cam->io = IO_NONE;
  1724. return -EFAULT;
  1725. }
  1726. cam->io = rb.count ? IO_MMAP : IO_NONE;
  1727. return 0;
  1728. }
  1729. static int
  1730. et61x251_vidioc_querybuf(struct et61x251_device* cam, void __user * arg)
  1731. {
  1732. struct v4l2_buffer b;
  1733. if (copy_from_user(&b, arg, sizeof(b)))
  1734. return -EFAULT;
  1735. if (b.type != V4L2_BUF_TYPE_VIDEO_CAPTURE ||
  1736. b.index >= cam->nbuffers || cam->io != IO_MMAP)
  1737. return -EINVAL;
  1738. memcpy(&b, &cam->frame[b.index].buf, sizeof(b));
  1739. if (cam->frame[b.index].vma_use_count)
  1740. b.flags |= V4L2_BUF_FLAG_MAPPED;
  1741. if (cam->frame[b.index].state == F_DONE)
  1742. b.flags |= V4L2_BUF_FLAG_DONE;
  1743. else if (cam->frame[b.index].state != F_UNUSED)
  1744. b.flags |= V4L2_BUF_FLAG_QUEUED;
  1745. if (copy_to_user(arg, &b, sizeof(b)))
  1746. return -EFAULT;
  1747. return 0;
  1748. }
  1749. static int
  1750. et61x251_vidioc_qbuf(struct et61x251_device* cam, void __user * arg)
  1751. {
  1752. struct v4l2_buffer b;
  1753. unsigned long lock_flags;
  1754. if (copy_from_user(&b, arg, sizeof(b)))
  1755. return -EFAULT;
  1756. if (b.type != V4L2_BUF_TYPE_VIDEO_CAPTURE ||
  1757. b.index >= cam->nbuffers || cam->io != IO_MMAP)
  1758. return -EINVAL;
  1759. if (cam->frame[b.index].state != F_UNUSED)
  1760. return -EINVAL;
  1761. cam->frame[b.index].state = F_QUEUED;
  1762. spin_lock_irqsave(&cam->queue_lock, lock_flags);
  1763. list_add_tail(&cam->frame[b.index].frame, &cam->inqueue);
  1764. spin_unlock_irqrestore(&cam->queue_lock, lock_flags);
  1765. PDBGG("Frame #%lu queued", (unsigned long)b.index);
  1766. return 0;
  1767. }
  1768. static int
  1769. et61x251_vidioc_dqbuf(struct et61x251_device* cam, struct file* filp,
  1770. void __user * arg)
  1771. {
  1772. struct v4l2_buffer b;
  1773. struct et61x251_frame_t *f;
  1774. unsigned long lock_flags;
  1775. long timeout;
  1776. if (copy_from_user(&b, arg, sizeof(b)))
  1777. return -EFAULT;
  1778. if (b.type != V4L2_BUF_TYPE_VIDEO_CAPTURE || cam->io!= IO_MMAP)
  1779. return -EINVAL;
  1780. if (list_empty(&cam->outqueue)) {
  1781. if (cam->stream == STREAM_OFF)
  1782. return -EINVAL;
  1783. if (filp->f_flags & O_NONBLOCK)
  1784. return -EAGAIN;
  1785. timeout = wait_event_interruptible_timeout
  1786. ( cam->wait_frame,
  1787. (!list_empty(&cam->outqueue)) ||
  1788. (cam->state & DEV_DISCONNECTED) ||
  1789. (cam->state & DEV_MISCONFIGURED),
  1790. cam->module_param.frame_timeout *
  1791. 1000 * msecs_to_jiffies(1) );
  1792. if (timeout < 0)
  1793. return timeout;
  1794. if (cam->state & DEV_DISCONNECTED)
  1795. return -ENODEV;
  1796. if (!timeout || (cam->state & DEV_MISCONFIGURED))
  1797. return -EIO;
  1798. }
  1799. spin_lock_irqsave(&cam->queue_lock, lock_flags);
  1800. f = list_entry(cam->outqueue.next, struct et61x251_frame_t, frame);
  1801. list_del(cam->outqueue.next);
  1802. spin_unlock_irqrestore(&cam->queue_lock, lock_flags);
  1803. f->state = F_UNUSED;
  1804. memcpy(&b, &f->buf, sizeof(b));
  1805. if (f->vma_use_count)
  1806. b.flags |= V4L2_BUF_FLAG_MAPPED;
  1807. if (copy_to_user(arg, &b, sizeof(b)))
  1808. return -EFAULT;
  1809. PDBGG("Frame #%lu dequeued", (unsigned long)f->buf.index);
  1810. return 0;
  1811. }
  1812. static int
  1813. et61x251_vidioc_streamon(struct et61x251_device* cam, void __user * arg)
  1814. {
  1815. int type;
  1816. if (copy_from_user(&type, arg, sizeof(type)))
  1817. return -EFAULT;
  1818. if (type != V4L2_BUF_TYPE_VIDEO_CAPTURE || cam->io != IO_MMAP)
  1819. return -EINVAL;
  1820. cam->stream = STREAM_ON;
  1821. DBG(3, "Stream on");
  1822. return 0;
  1823. }
  1824. static int
  1825. et61x251_vidioc_streamoff(struct et61x251_device* cam, void __user * arg)
  1826. {
  1827. int type, err;
  1828. if (copy_from_user(&type, arg, sizeof(type)))
  1829. return -EFAULT;
  1830. if (type != V4L2_BUF_TYPE_VIDEO_CAPTURE || cam->io != IO_MMAP)
  1831. return -EINVAL;
  1832. if (cam->stream == STREAM_ON)
  1833. if ((err = et61x251_stream_interrupt(cam)))
  1834. return err;
  1835. et61x251_empty_framequeues(cam);
  1836. DBG(3, "Stream off");
  1837. return 0;
  1838. }
  1839. static int
  1840. et61x251_vidioc_g_parm(struct et61x251_device* cam, void __user * arg)
  1841. {
  1842. struct v4l2_streamparm sp;
  1843. if (copy_from_user(&sp, arg, sizeof(sp)))
  1844. return -EFAULT;
  1845. if (sp.type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
  1846. return -EINVAL;
  1847. sp.parm.capture.extendedmode = 0;
  1848. sp.parm.capture.readbuffers = cam->nreadbuffers;
  1849. if (copy_to_user(arg, &sp, sizeof(sp)))
  1850. return -EFAULT;
  1851. return 0;
  1852. }
  1853. static int
  1854. et61x251_vidioc_s_parm(struct et61x251_device* cam, void __user * arg)
  1855. {
  1856. struct v4l2_streamparm sp;
  1857. if (copy_from_user(&sp, arg, sizeof(sp)))
  1858. return -EFAULT;
  1859. if (sp.type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
  1860. return -EINVAL;
  1861. sp.parm.capture.extendedmode = 0;
  1862. if (sp.parm.capture.readbuffers == 0)
  1863. sp.parm.capture.readbuffers = cam->nreadbuffers;
  1864. if (sp.parm.capture.readbuffers > ET61X251_MAX_FRAMES)
  1865. sp.parm.capture.readbuffers = ET61X251_MAX_FRAMES;
  1866. if (copy_to_user(arg, &sp, sizeof(sp)))
  1867. return -EFAULT;
  1868. cam->nreadbuffers = sp.parm.capture.readbuffers;
  1869. return 0;
  1870. }
  1871. static int et61x251_ioctl_v4l2(struct inode* inode, struct file* filp,
  1872. unsigned int cmd, void __user * arg)
  1873. {
  1874. struct et61x251_device* cam = video_get_drvdata(video_devdata(filp));
  1875. switch (cmd) {
  1876. case VIDIOC_QUERYCAP:
  1877. return et61x251_vidioc_querycap(cam, arg);
  1878. case VIDIOC_ENUMINPUT:
  1879. return et61x251_vidioc_enuminput(cam, arg);
  1880. case VIDIOC_G_INPUT:
  1881. return et61x251_vidioc_g_input(cam, arg);
  1882. case VIDIOC_S_INPUT:
  1883. return et61x251_vidioc_s_input(cam, arg);
  1884. case VIDIOC_QUERYCTRL:
  1885. return et61x251_vidioc_query_ctrl(cam, arg);
  1886. case VIDIOC_G_CTRL:
  1887. return et61x251_vidioc_g_ctrl(cam, arg);
  1888. case VIDIOC_S_CTRL:
  1889. return et61x251_vidioc_s_ctrl(cam, arg);
  1890. case VIDIOC_CROPCAP:
  1891. return et61x251_vidioc_cropcap(cam, arg);
  1892. case VIDIOC_G_CROP:
  1893. return et61x251_vidioc_g_crop(cam, arg);
  1894. case VIDIOC_S_CROP:
  1895. return et61x251_vidioc_s_crop(cam, arg);
  1896. case VIDIOC_ENUM_FMT:
  1897. return et61x251_vidioc_enum_fmt(cam, arg);
  1898. case VIDIOC_G_FMT:
  1899. return et61x251_vidioc_g_fmt(cam, arg);
  1900. case VIDIOC_TRY_FMT:
  1901. case VIDIOC_S_FMT:
  1902. return et61x251_vidioc_try_s_fmt(cam, cmd, arg);
  1903. case VIDIOC_ENUM_FRAMESIZES:
  1904. return et61x251_vidioc_enum_framesizes(cam, arg);
  1905. case VIDIOC_G_JPEGCOMP:
  1906. return et61x251_vidioc_g_jpegcomp(cam, arg);
  1907. case VIDIOC_S_JPEGCOMP:
  1908. return et61x251_vidioc_s_jpegcomp(cam, arg);
  1909. case VIDIOC_REQBUFS:
  1910. return et61x251_vidioc_reqbufs(cam, arg);
  1911. case VIDIOC_QUERYBUF:
  1912. return et61x251_vidioc_querybuf(cam, arg);
  1913. case VIDIOC_QBUF:
  1914. return et61x251_vidioc_qbuf(cam, arg);
  1915. case VIDIOC_DQBUF:
  1916. return et61x251_vidioc_dqbuf(cam, filp, arg);
  1917. case VIDIOC_STREAMON:
  1918. return et61x251_vidioc_streamon(cam, arg);
  1919. case VIDIOC_STREAMOFF:
  1920. return et61x251_vidioc_streamoff(cam, arg);
  1921. case VIDIOC_G_PARM:
  1922. return et61x251_vidioc_g_parm(cam, arg);
  1923. case VIDIOC_S_PARM:
  1924. return et61x251_vidioc_s_parm(cam, arg);
  1925. case VIDIOC_G_STD:
  1926. case VIDIOC_S_STD:
  1927. case VIDIOC_QUERYSTD:
  1928. case VIDIOC_ENUMSTD:
  1929. case VIDIOC_QUERYMENU:
  1930. case VIDIOC_ENUM_FRAMEINTERVALS:
  1931. return -EINVAL;
  1932. default:
  1933. return -EINVAL;
  1934. }
  1935. }
  1936. static int et61x251_ioctl(struct inode* inode, struct file* filp,
  1937. unsigned int cmd, unsigned long arg)
  1938. {
  1939. struct et61x251_device* cam = video_get_drvdata(video_devdata(filp));
  1940. int err = 0;
  1941. if (mutex_lock_interruptible(&cam->fileop_mutex))
  1942. return -ERESTARTSYS;
  1943. if (cam->state & DEV_DISCONNECTED) {
  1944. DBG(1, "Device not present");
  1945. mutex_unlock(&cam->fileop_mutex);
  1946. return -ENODEV;
  1947. }
  1948. if (cam->state & DEV_MISCONFIGURED) {
  1949. DBG(1, "The camera is misconfigured. Close and open it "
  1950. "again.");
  1951. mutex_unlock(&cam->fileop_mutex);
  1952. return -EIO;
  1953. }
  1954. V4LDBG(3, "et61x251", cmd);
  1955. err = et61x251_ioctl_v4l2(inode, filp, cmd, (void __user *)arg);
  1956. mutex_unlock(&cam->fileop_mutex);
  1957. return err;
  1958. }
  1959. static const struct file_operations et61x251_fops = {
  1960. .owner = THIS_MODULE,
  1961. .open = et61x251_open,
  1962. .release = et61x251_release,
  1963. .ioctl = et61x251_ioctl,
  1964. .compat_ioctl = v4l_compat_ioctl32,
  1965. .read = et61x251_read,
  1966. .poll = et61x251_poll,
  1967. .mmap = et61x251_mmap,
  1968. .llseek = no_llseek,
  1969. };
  1970. /*****************************************************************************/
  1971. /* It exists a single interface only. We do not need to validate anything. */
  1972. static int
  1973. et61x251_usb_probe(struct usb_interface* intf, const struct usb_device_id* id)
  1974. {
  1975. struct usb_device *udev = interface_to_usbdev(intf);
  1976. struct et61x251_device* cam;
  1977. static unsigned int dev_nr = 0;
  1978. unsigned int i;
  1979. int err = 0;
  1980. if (!(cam = kzalloc(sizeof(struct et61x251_device), GFP_KERNEL)))
  1981. return -ENOMEM;
  1982. cam->usbdev = udev;
  1983. if (!(cam->control_buffer = kzalloc(8, GFP_KERNEL))) {
  1984. DBG(1, "kmalloc() failed");
  1985. err = -ENOMEM;
  1986. goto fail;
  1987. }
  1988. if (!(cam->v4ldev = video_device_alloc())) {
  1989. DBG(1, "video_device_alloc() failed");
  1990. err = -ENOMEM;
  1991. goto fail;
  1992. }
  1993. DBG(2, "ET61X[12]51 PC Camera Controller detected "
  1994. "(vid/pid 0x%04X:0x%04X)",id->idVendor, id->idProduct);
  1995. for (i = 0; et61x251_sensor_table[i]; i++) {
  1996. err = et61x251_sensor_table[i](cam);
  1997. if (!err)
  1998. break;
  1999. }
  2000. if (!err)
  2001. DBG(2, "%s image sensor detected", cam->sensor.name);
  2002. else {
  2003. DBG(1, "No supported image sensor detected");
  2004. err = -ENODEV;
  2005. goto fail;
  2006. }
  2007. if (et61x251_init(cam)) {
  2008. DBG(1, "Initialization failed. I will retry on open().");
  2009. cam->state |= DEV_MISCONFIGURED;
  2010. }
  2011. strcpy(cam->v4ldev->name, "ET61X[12]51 PC Camera");
  2012. cam->v4ldev->owner = THIS_MODULE;
  2013. cam->v4ldev->type = VID_TYPE_CAPTURE | VID_TYPE_SCALES;
  2014. cam->v4ldev->hardware = 0;
  2015. cam->v4ldev->fops = &et61x251_fops;
  2016. cam->v4ldev->minor = video_nr[dev_nr];
  2017. cam->v4ldev->release = video_device_release;
  2018. video_set_drvdata(cam->v4ldev, cam);
  2019. init_completion(&cam->probe);
  2020. err = video_register_device(cam->v4ldev, VFL_TYPE_GRABBER,
  2021. video_nr[dev_nr]);
  2022. if (err) {
  2023. DBG(1, "V4L2 device registration failed");
  2024. if (err == -ENFILE && video_nr[dev_nr] == -1)
  2025. DBG(1, "Free /dev/videoX node not found");
  2026. video_nr[dev_nr] = -1;
  2027. dev_nr = (dev_nr < ET61X251_MAX_DEVICES-1) ? dev_nr+1 : 0;
  2028. complete_all(&cam->probe);
  2029. goto fail;
  2030. }
  2031. DBG(2, "V4L2 device registered as /dev/video%d", cam->v4ldev->minor);
  2032. cam->module_param.force_munmap = force_munmap[dev_nr];
  2033. cam->module_param.frame_timeout = frame_timeout[dev_nr];
  2034. dev_nr = (dev_nr < ET61X251_MAX_DEVICES-1) ? dev_nr+1 : 0;
  2035. #ifdef CONFIG_VIDEO_ADV_DEBUG
  2036. err = et61x251_create_sysfs(cam);
  2037. if (!err)
  2038. DBG(2, "Optional device control through 'sysfs' "
  2039. "interface ready");
  2040. else
  2041. DBG(2, "Failed to create 'sysfs' interface for optional "
  2042. "device controlling. Error #%d", err);
  2043. #else
  2044. DBG(2, "Optional device control through 'sysfs' interface disabled");
  2045. DBG(3, "Compile the kernel with the 'CONFIG_VIDEO_ADV_DEBUG' "
  2046. "configuration option to enable it.");
  2047. #endif
  2048. usb_set_intfdata(intf, cam);
  2049. kref_init(&cam->kref);
  2050. usb_get_dev(cam->usbdev);
  2051. complete_all(&cam->probe);
  2052. return 0;
  2053. fail:
  2054. if (cam) {
  2055. kfree(cam->control_buffer);
  2056. if (cam->v4ldev)
  2057. video_device_release(cam->v4ldev);
  2058. kfree(cam);
  2059. }
  2060. return err;
  2061. }
  2062. static void et61x251_usb_disconnect(struct usb_interface* intf)
  2063. {
  2064. struct et61x251_device* cam;
  2065. down_write(&et61x251_dev_lock);
  2066. cam = usb_get_intfdata(intf);
  2067. DBG(2, "Disconnecting %s...", cam->v4ldev->name);
  2068. if (cam->users) {
  2069. DBG(2, "Device /dev/video%d is open! Deregistration and "
  2070. "memory deallocation are deferred.",
  2071. cam->v4ldev->minor);
  2072. cam->state |= DEV_MISCONFIGURED;
  2073. et61x251_stop_transfer(cam);
  2074. cam->state |= DEV_DISCONNECTED;
  2075. wake_up_interruptible(&cam->wait_frame);
  2076. wake_up(&cam->wait_stream);
  2077. } else
  2078. cam->state |= DEV_DISCONNECTED;
  2079. wake_up_interruptible_all(&cam->wait_open);
  2080. kref_put(&cam->kref, et61x251_release_resources);
  2081. up_write(&et61x251_dev_lock);
  2082. }
  2083. static struct usb_driver et61x251_usb_driver = {
  2084. .name = "et61x251",
  2085. .id_table = et61x251_id_table,
  2086. .probe = et61x251_usb_probe,
  2087. .disconnect = et61x251_usb_disconnect,
  2088. };
  2089. /*****************************************************************************/
  2090. static int __init et61x251_module_init(void)
  2091. {
  2092. int err = 0;
  2093. KDBG(2, ET61X251_MODULE_NAME " v" ET61X251_MODULE_VERSION);
  2094. KDBG(3, ET61X251_MODULE_AUTHOR);
  2095. if ((err = usb_register(&et61x251_usb_driver)))
  2096. KDBG(1, "usb_register() failed");
  2097. return err;
  2098. }
  2099. static void __exit et61x251_module_exit(void)
  2100. {
  2101. usb_deregister(&et61x251_usb_driver);
  2102. }
  2103. module_init(et61x251_module_init);
  2104. module_exit(et61x251_module_exit);