video.c 62 KB

12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758596061626364656667686970717273747576777879808182838485868788899091929394959697989910010110210310410510610710810911011111211311411511611711811912012112212312412512612712812913013113213313413513613713813914014114214314414514614714814915015115215315415515615715815916016116216316416516616716816917017117217317417517617717817918018118218318418518618718818919019119219319419519619719819920020120220320420520620720820921021121221321421521621721821922022122222322422522622722822923023123223323423523623723823924024124224324424524624724824925025125225325425525625725825926026126226326426526626726826927027127227327427527627727827928028128228328428528628728828929029129229329429529629729829930030130230330430530630730830931031131231331431531631731831932032132232332432532632732832933033133233333433533633733833934034134234334434534634734834935035135235335435535635735835936036136236336436536636736836937037137237337437537637737837938038138238338438538638738838939039139239339439539639739839940040140240340440540640740840941041141241341441541641741841942042142242342442542642742842943043143243343443543643743843944044144244344444544644744844945045145245345445545645745845946046146246346446546646746846947047147247347447547647747847948048148248348448548648748848949049149249349449549649749849950050150250350450550650750850951051151251351451551651751851952052152252352452552652752852953053153253353453553653753853954054154254354454554654754854955055155255355455555655755855956056156256356456556656756856957057157257357457557657757857958058158258358458558658758858959059159259359459559659759859960060160260360460560660760860961061161261361461561661761861962062162262362462562662762862963063163263363463563663763863964064164264364464564664764864965065165265365465565665765865966066166266366466566666766866967067167267367467567667767867968068168268368468568668768868969069169269369469569669769869970070170270370470570670770870971071171271371471571671771871972072172272372472572672772872973073173273373473573673773873974074174274374474574674774874975075175275375475575675775875976076176276376476576676776876977077177277377477577677777877978078178278378478578678778878979079179279379479579679779879980080180280380480580680780880981081181281381481581681781881982082182282382482582682782882983083183283383483583683783883984084184284384484584684784884985085185285385485585685785885986086186286386486586686786886987087187287387487587687787887988088188288388488588688788888989089189289389489589689789889990090190290390490590690790890991091191291391491591691791891992092192292392492592692792892993093193293393493593693793893994094194294394494594694794894995095195295395495595695795895996096196296396496596696796896997097197297397497597697797897998098198298398498598698798898999099199299399499599699799899910001001100210031004100510061007100810091010101110121013101410151016101710181019102010211022102310241025102610271028102910301031103210331034103510361037103810391040104110421043104410451046104710481049105010511052105310541055105610571058105910601061106210631064106510661067106810691070107110721073107410751076107710781079108010811082108310841085108610871088108910901091109210931094109510961097109810991100110111021103110411051106110711081109111011111112111311141115111611171118111911201121112211231124112511261127112811291130113111321133113411351136113711381139114011411142114311441145114611471148114911501151115211531154115511561157115811591160116111621163116411651166116711681169117011711172117311741175117611771178117911801181118211831184118511861187118811891190119111921193119411951196119711981199120012011202120312041205120612071208120912101211121212131214121512161217121812191220122112221223122412251226122712281229123012311232123312341235123612371238123912401241124212431244124512461247124812491250125112521253125412551256125712581259126012611262126312641265126612671268126912701271127212731274127512761277127812791280128112821283128412851286128712881289129012911292129312941295129612971298129913001301130213031304130513061307130813091310131113121313131413151316131713181319132013211322132313241325132613271328132913301331133213331334133513361337133813391340134113421343134413451346134713481349135013511352135313541355135613571358135913601361136213631364136513661367136813691370137113721373137413751376137713781379138013811382138313841385138613871388138913901391139213931394139513961397139813991400140114021403140414051406140714081409141014111412141314141415141614171418141914201421142214231424142514261427142814291430143114321433143414351436143714381439144014411442144314441445144614471448144914501451145214531454145514561457145814591460146114621463146414651466146714681469147014711472147314741475147614771478147914801481148214831484148514861487148814891490149114921493149414951496149714981499150015011502150315041505150615071508150915101511151215131514151515161517151815191520152115221523152415251526152715281529153015311532153315341535153615371538153915401541154215431544154515461547154815491550155115521553155415551556155715581559156015611562156315641565156615671568156915701571157215731574157515761577157815791580158115821583158415851586158715881589159015911592159315941595159615971598159916001601160216031604160516061607160816091610161116121613161416151616161716181619162016211622162316241625162616271628162916301631163216331634163516361637163816391640164116421643164416451646164716481649165016511652165316541655165616571658165916601661166216631664166516661667166816691670167116721673167416751676167716781679168016811682168316841685168616871688168916901691169216931694169516961697169816991700170117021703170417051706170717081709171017111712171317141715171617171718171917201721172217231724172517261727172817291730173117321733173417351736173717381739174017411742174317441745174617471748174917501751175217531754175517561757175817591760176117621763176417651766176717681769177017711772177317741775177617771778177917801781178217831784178517861787178817891790179117921793179417951796179717981799180018011802180318041805180618071808180918101811181218131814181518161817181818191820182118221823182418251826182718281829183018311832183318341835183618371838183918401841184218431844184518461847184818491850185118521853185418551856185718581859186018611862186318641865186618671868186918701871187218731874187518761877187818791880188118821883188418851886188718881889189018911892189318941895189618971898189919001901190219031904190519061907190819091910191119121913191419151916191719181919192019211922192319241925192619271928192919301931193219331934193519361937193819391940194119421943194419451946194719481949195019511952195319541955195619571958195919601961196219631964196519661967196819691970197119721973197419751976197719781979198019811982198319841985198619871988198919901991199219931994199519961997199819992000200120022003200420052006200720082009201020112012201320142015201620172018201920202021202220232024202520262027202820292030203120322033203420352036203720382039204020412042204320442045204620472048204920502051205220532054205520562057205820592060206120622063206420652066206720682069207020712072207320742075207620772078207920802081208220832084208520862087208820892090209120922093209420952096209720982099210021012102210321042105210621072108210921102111211221132114211521162117211821192120212121222123212421252126212721282129213021312132213321342135213621372138213921402141214221432144214521462147214821492150215121522153215421552156215721582159216021612162216321642165216621672168216921702171217221732174217521762177217821792180218121822183218421852186218721882189219021912192219321942195219621972198219922002201220222032204220522062207220822092210221122122213221422152216221722182219222022212222222322242225222622272228222922302231223222332234223522362237223822392240224122422243224422452246224722482249225022512252225322542255225622572258225922602261226222632264226522662267226822692270227122722273227422752276227722782279228022812282228322842285228622872288228922902291229222932294229522962297229822992300230123022303230423052306230723082309231023112312231323142315231623172318231923202321232223232324232523262327232823292330233123322333233423352336233723382339234023412342234323442345234623472348234923502351235223532354235523562357235823592360236123622363236423652366236723682369237023712372237323742375237623772378237923802381238223832384238523862387
  1. /****************************************************************************
  2. *
  3. * The MIT License (MIT)
  4. *
  5. * Copyright (c) 2020 VeriSilicon Holdings Co., Ltd.
  6. *
  7. * Permission is hereby granted, free of charge, to any person obtaining a
  8. * copy of this software and associated documentation files (the "Software"),
  9. * to deal in the Software without restriction, including without limitation
  10. * the rights to use, copy, modify, merge, publish, distribute, sublicense,
  11. * and/or sell copies of the Software, and to permit persons to whom the
  12. * Software is furnished to do so, subject to the following conditions:
  13. *
  14. * The above copyright notice and this permission notice shall be included in
  15. * all copies or substantial portions of the Software.
  16. *
  17. * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  18. * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  19. * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
  20. * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
  21. * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
  22. * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
  23. * DEALINGS IN THE SOFTWARE.
  24. *
  25. *****************************************************************************
  26. *
  27. * The GPL License (GPL)
  28. *
  29. * Copyright (c) 2020 VeriSilicon Holdings Co., Ltd.
  30. *
  31. * This program is free software; you can redistribute it and/or
  32. * modify it under the terms of the GNU General Public License
  33. * as published by the Free Software Foundation; either version 2
  34. * of the License, or (at your option) any later version.
  35. *
  36. * This program is distributed in the hope that it will be useful,
  37. * but WITHOUT ANY WARRANTY; without even the implied warranty of
  38. * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  39. * GNU General Public License for more details.
  40. *
  41. * You should have received a copy of the GNU General Public License
  42. * along with this program;
  43. *
  44. *****************************************************************************
  45. *
  46. * Note: This software is released under dual MIT and GPL licenses. A
  47. * recipient may use this file under the terms of either the MIT license or
  48. * GPL License. If you wish to use only one license not the other, you can
  49. * indicate your decision by deleting one of the above license notices in your
  50. * version of this file.
  51. *
  52. *****************************************************************************/
  53. #ifdef CONFIG_VIDEOBUF2_DMA_CONTIG
  54. # include <linux/dma-direct.h>
  55. #endif
  56. #include <linux/module.h>
  57. #include <linux/platform_device.h>
  58. #include <linux/spinlock.h>
  59. #include <linux/version.h>
  60. #include <media/v4l2-device.h>
  61. #include <media/v4l2-event.h>
  62. #include <media/v4l2-fh.h>
  63. #include <media/v4l2-ioctl.h>
  64. #include <media/videobuf2-dma-contig.h>
  65. #include <linux/of_reserved_mem.h>
  66. #include "video.h"
  67. #include "vvctrl.h"
  68. #include "vvdefs.h"
  69. #include "vvsensor.h"
  70. #define DEF_PLANE_NO (0)
  71. #define RETRY_TIME_INTERVAL_MS (5)
  72. #define RETRY_TIMES_MAX (10)
  73. static struct viv_video_device *vvdev[VIDEO_NODE_NUM];
  74. static struct list_head file_list_head[VIDEO_NODE_NUM];
  75. static spinlock_t file_list_lock[VIDEO_NODE_NUM];
  76. #ifdef ENABLE_IRQ
  77. static struct media_device mdev;
  78. #endif
  79. #ifdef CONFIG_VIDEOBUF2_DMA_CONTIG
  80. struct ext_dma_buf {
  81. dma_addr_t addr;
  82. void *vaddr;
  83. size_t size;
  84. struct list_head entry;
  85. };
  86. #endif
  87. static struct viv_video_fmt formats[] = {
  88. {
  89. .fourcc = V4L2_PIX_FMT_YUYV,
  90. .depth = 16,
  91. .bpp = 2,
  92. },
  93. {
  94. .fourcc = V4L2_PIX_FMT_NV12,
  95. .depth = 12,
  96. .bpp = 1,
  97. },
  98. {
  99. .fourcc = V4L2_PIX_FMT_NV16,
  100. .depth = 16,
  101. .bpp = 1,
  102. },
  103. };
  104. static int bayer_pattern_to_format(unsigned int bayer_pattern,
  105. unsigned int bit_width, struct viv_video_fmt *fmt)
  106. {
  107. int ret = 0;
  108. if (bayer_pattern == BAYER_BGGR)
  109. if (bit_width == 8) {
  110. fmt->fourcc = V4L2_PIX_FMT_SBGGR8;
  111. fmt->depth = 8;
  112. fmt->bpp = 1;
  113. } else if (bit_width == 10) {
  114. fmt->fourcc = V4L2_PIX_FMT_SBGGR10;
  115. fmt->depth = 16;
  116. fmt->bpp = 2;
  117. } else if (bit_width == 12) {
  118. fmt->fourcc = V4L2_PIX_FMT_SBGGR12;
  119. fmt->depth = 16;
  120. fmt->bpp = 2;
  121. } else
  122. ret = -EPERM;
  123. else if (bayer_pattern == BAYER_GBRG)
  124. if (bit_width == 8) {
  125. fmt->fourcc = V4L2_PIX_FMT_SGBRG8;
  126. fmt->depth = 8;
  127. fmt->bpp = 1;
  128. } else if (bit_width == 10) {
  129. fmt->fourcc = V4L2_PIX_FMT_SGBRG10;
  130. fmt->depth = 16;
  131. fmt->bpp = 2;
  132. } else if (bit_width == 12) {
  133. fmt->fourcc = V4L2_PIX_FMT_SGBRG12;
  134. fmt->depth = 16;
  135. fmt->bpp = 2;
  136. } else
  137. ret = -EPERM;
  138. else if (bayer_pattern == BAYER_GRBG)
  139. if (bit_width == 8) {
  140. fmt->fourcc = V4L2_PIX_FMT_SGRBG8;
  141. fmt->depth = 8;
  142. fmt->bpp = 1;
  143. } else if (bit_width == 10) {
  144. fmt->fourcc = V4L2_PIX_FMT_SGRBG10;
  145. fmt->depth = 16;
  146. fmt->bpp = 2;
  147. } else if (bit_width == 12) {
  148. fmt->fourcc = V4L2_PIX_FMT_SGRBG12;
  149. fmt->depth = 16;
  150. fmt->bpp = 2;
  151. } else
  152. ret = -EPERM;
  153. else if (bayer_pattern == BAYER_RGGB)
  154. if (bit_width == 8) {
  155. fmt->fourcc = V4L2_PIX_FMT_SRGGB8;
  156. fmt->depth = 8;
  157. fmt->bpp = 1;
  158. } else if (bit_width == 10) {
  159. fmt->fourcc = V4L2_PIX_FMT_SRGGB10;
  160. fmt->depth = 16;
  161. fmt->bpp = 2;
  162. } else if (bit_width == 12) {
  163. fmt->fourcc = V4L2_PIX_FMT_SRGGB12;
  164. fmt->depth = 16;
  165. fmt->bpp = 2;
  166. } else
  167. ret = -EPERM;
  168. else
  169. ret = -EPERM;
  170. return ret;
  171. }
  172. /* Caller must hold fh->vdev->fh_lock! */
  173. static struct v4l2_subscribed_event *video_event_fh_subscribed(
  174. struct v4l2_fh *fh, u32 type, u32 id)
  175. {
  176. struct v4l2_subscribed_event *sev;
  177. assert_spin_locked(&fh->vdev->fh_lock);
  178. list_for_each_entry(sev, &fh->subscribed, list)
  179. if (sev->type == type && sev->id == id) {
  180. return sev;
  181. }
  182. return NULL;
  183. }
  184. /*0-unsubscribed 1-subscribed*/
  185. static int video_event_subscribed(struct video_device *vdev,
  186. const struct v4l2_event *ev, u16 retries)
  187. {
  188. struct v4l2_fh *fh;
  189. unsigned long flags;
  190. int retry;
  191. if (vdev == NULL || ev == NULL)
  192. return 0;
  193. /*retry for waitting the daemon to subscribe */
  194. for(retry = 0; retry < retries; retry++) {
  195. spin_lock_irqsave(&vdev->fh_lock, flags);
  196. /*search the fh_list and check the event subscribed or not*/
  197. list_for_each_entry(fh, &vdev->fh_list, list) {
  198. if(video_event_fh_subscribed(fh, ev->type, ev->id)) {
  199. spin_unlock_irqrestore(&vdev->fh_lock, flags);
  200. return 1;
  201. }
  202. }
  203. spin_unlock_irqrestore(&vdev->fh_lock, flags);
  204. msleep(RETRY_TIME_INTERVAL_MS);
  205. }
  206. return 0;
  207. }
  208. static int viv_post_event(struct v4l2_event *event, void *fh, bool sync)
  209. {
  210. struct viv_video_file *handle = priv_to_handle(fh);
  211. struct v4l2_fh *video_fh = (struct v4l2_fh *)fh;
  212. if(!video_fh || !event) {
  213. return -EINVAL;
  214. }
  215. if(!video_event_subscribed(video_fh->vdev, event, RETRY_TIMES_MAX)) {
  216. pr_err("%s: unsubscribed event id =%d type=0x%08x",
  217. __func__, event->id, event->type);
  218. return -EAGAIN;
  219. }
  220. if (sync)
  221. reinit_completion(&handle->wait);
  222. mutex_lock(&handle->event_mutex);
  223. v4l2_event_queue(handle->vdev->video, event);
  224. mutex_unlock(&handle->event_mutex);
  225. if (sync) {
  226. if (wait_for_completion_timeout(&handle->wait, msecs_to_jiffies(
  227. VIV_VIDEO_EVENT_TIMOUT_MS)) == 0)
  228. return -ETIMEDOUT;
  229. }
  230. return 0;
  231. }
  232. static int viv_post_simple_event(int id, int streamid, void *fh, bool sync)
  233. {
  234. struct v4l2_event event;
  235. struct viv_video_event *v_event;
  236. v_event = (struct viv_video_event *)&event.u.data[0];
  237. v_event->stream_id = streamid;
  238. v_event->file = fh;
  239. v_event->sync = sync;
  240. event.type = VIV_VIDEO_EVENT_TYPE;
  241. event.id = id;
  242. return viv_post_event(&event, fh, sync);
  243. }
  244. static int viv_post_control_event(int streamid, void *fh,
  245. struct viv_control_event *control_event)
  246. {
  247. struct v4l2_event event;
  248. struct viv_video_event *v_event;
  249. v_event = (struct viv_video_event *)&event.u.data[0];
  250. v_event->stream_id = streamid;
  251. v_event->file = fh;
  252. v_event->sync = true;
  253. v_event->addr = control_event->request;
  254. v_event->response = control_event->response;
  255. v_event->buf_index = control_event->id;
  256. event.type = VIV_VIDEO_EVENT_TYPE;
  257. event.id = VIV_VIDEO_EVENT_PASS_JSON;
  258. return viv_post_event(&event, fh, true);
  259. }
  260. static int set_stream(struct viv_video_device *vdev, int enable)
  261. {
  262. struct v4l2_subdev *sd;
  263. struct media_pad *pad;
  264. if (!vdev)
  265. return -EINVAL;
  266. pad = &vdev->video->entity.pads[0];
  267. if (pad)
  268. pad = media_entity_remote_pad(pad);
  269. if (pad && is_media_entity_v4l2_subdev(pad->entity)) {
  270. sd = media_entity_to_v4l2_subdev(pad->entity);
  271. v4l2_subdev_call(sd, video, s_stream, enable);
  272. }
  273. return 0;
  274. }
  275. static int start_streaming(struct vb2_queue *vq, unsigned int count)
  276. {
  277. struct viv_video_file *handle = queue_to_handle(vq);
  278. struct v4l2_fh *fh = &handle->vfh;
  279. pr_debug("enter %s\n", __func__);
  280. if (handle->streamid >= 0 && handle->state != 2) {
  281. handle->state = 2;
  282. handle->vdev->active = 1;
  283. set_stream(handle->vdev, 1);
  284. viv_post_simple_event(VIV_VIDEO_EVENT_START_STREAM,
  285. handle->streamid, fh, true);
  286. } else {
  287. pr_err("can't start streaming, device busy!\n");
  288. return -EBUSY;
  289. }
  290. return 0;
  291. }
  292. static void stop_streaming(struct vb2_queue *vq)
  293. {
  294. struct viv_video_file *handle = queue_to_handle(vq);
  295. struct vb2_buffer *vb;
  296. pr_debug("enter %s\n", __func__);
  297. if (!handle || handle->streamid < 0 || handle->state != 2)
  298. return;
  299. handle->state = 1;
  300. set_stream(handle->vdev, 0);
  301. viv_post_simple_event(VIV_VIDEO_EVENT_STOP_STREAM, handle->streamid,
  302. &handle->vfh, true);
  303. handle->sequence = 0;
  304. list_for_each_entry(vb, &vq->queued_list, queued_entry) {
  305. if (vb->state == VB2_BUF_STATE_ACTIVE)
  306. vb2_buffer_done(vb, VB2_BUF_STATE_ERROR);
  307. }
  308. }
  309. #if LINUX_VERSION_CODE < KERNEL_VERSION(4, 8, 0)
  310. static int queue_setup(struct vb2_queue *vq, const struct v4l2_format *fmt,
  311. unsigned int *nbuffers, unsigned int *nplanes,
  312. unsigned int sizes[], void *alloc_ctxs[])
  313. {
  314. struct viv_video_file *handle = queue_to_handle(vq);
  315. unsigned long size = handle->vdev->fmt.fmt.pix.sizeimage;
  316. pr_debug("enter %s\n", __func__);
  317. if (*nbuffers == 0)
  318. *nbuffers = 1;
  319. while (size * *nbuffers > RESERVED_MEM_SIZE)
  320. (*nbuffers)--;
  321. sizes[0] = size;
  322. return 0;
  323. }
  324. #else
  325. static int queue_setup(struct vb2_queue *q,
  326. unsigned int *num_buffers, unsigned int *num_planes,
  327. unsigned int sizes[], struct device *alloc_devs[])
  328. {
  329. struct viv_video_file *handle = queue_to_handle(q);
  330. unsigned long size = handle->vdev->fmt.fmt.pix.sizeimage;
  331. pr_debug("enter %s\n", __func__);
  332. if (*num_buffers == 0)
  333. *num_buffers = 1;
  334. while (size * *num_buffers > RESERVED_MEM_SIZE)
  335. (*num_buffers)--;
  336. *num_planes = 1;
  337. sizes[0] = size;
  338. return 0;
  339. }
  340. #endif
  341. static int buffer_init(struct vb2_buffer *vb)
  342. {
  343. pr_debug("enter %s\n", __func__);
  344. return 0;
  345. }
  346. static void buffer_queue(struct vb2_buffer *vb)
  347. {
  348. struct viv_video_file *handle;
  349. struct vb2_v4l2_buffer *vbuf;
  350. struct vb2_dc_buf *buf;
  351. #ifdef ENABLE_IRQ
  352. struct viv_video_device *vdev;
  353. struct media_pad *pad;
  354. #else
  355. struct v4l2_event event;
  356. struct viv_video_event *v_event;
  357. #endif
  358. if (!vb)
  359. return;
  360. handle = queue_to_handle(vb->vb2_queue);
  361. vbuf = container_of(vb, struct vb2_v4l2_buffer, vb2_buf);
  362. buf = container_of(vbuf, struct vb2_dc_buf, vb);
  363. if (!buf)
  364. return;
  365. if (!handle)
  366. return;
  367. #ifdef CONFIG_VIDEOBUF2_DMA_CONTIG
  368. buf->dma = vb2_dma_contig_plane_dma_addr(vb, DEF_PLANE_NO);
  369. #endif
  370. #ifdef ENABLE_IRQ
  371. vdev = handle->vdev;
  372. if (!vdev)
  373. return;
  374. pad = &vdev->video->entity.pads[0];
  375. vvbuf_ready(&vdev->bctx, pad, buf);
  376. #endif
  377. #ifndef ENABLE_IRQ
  378. if (handle->streamid < 0)
  379. return;
  380. /* pr_debug("buffer_queue %d", vb->index); */
  381. v_event = (struct viv_video_event *)&event.u.data[0];
  382. v_event->stream_id = handle->streamid;
  383. v_event->file = &handle->vfh;
  384. #ifdef CONFIG_VIDEOBUF2_DMA_CONTIG
  385. v_event->addr = vb2_dma_contig_plane_dma_addr(vb, DEF_PLANE_NO);
  386. #endif
  387. v_event->buf_index = vb->index;
  388. v_event->sync = false;
  389. event.type = VIV_VIDEO_EVENT_TYPE;
  390. event.id = VIV_VIDEO_EVENT_QBUF;
  391. viv_post_event(&event, &handle->vfh, false);
  392. #endif
  393. }
  394. static struct vb2_ops buffer_ops = {
  395. .queue_setup = queue_setup,
  396. .buf_init = buffer_init,
  397. .buf_queue = buffer_queue,
  398. .start_streaming = start_streaming,
  399. .stop_streaming = stop_streaming,
  400. };
  401. static int video_open(struct file *file)
  402. {
  403. struct viv_video_device *dev = video_drvdata(file);
  404. struct viv_video_file *handle;
  405. unsigned long flags;
  406. int rc;
  407. pr_debug("enter %s\n", __func__);
  408. handle = kzalloc(sizeof(*handle), GFP_KERNEL);
  409. v4l2_fh_init(&handle->vfh, dev->video);
  410. v4l2_fh_add(&handle->vfh);
  411. file->private_data = &handle->vfh;
  412. handle->vdev = dev;
  413. handle->queue.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  414. handle->queue.drv_priv = handle;
  415. handle->queue.ops = &buffer_ops;
  416. #ifdef CONFIG_VIDEOBUF2_DMA_CONTIG
  417. handle->queue.io_modes = VB2_MMAP | VB2_DMABUF;
  418. handle->queue.mem_ops = &vb2_dma_contig_memops;
  419. #endif
  420. handle->queue.buf_struct_size = sizeof(struct vb2_dc_buf);
  421. handle->queue.timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
  422. #if LINUX_VERSION_CODE > KERNEL_VERSION(4, 5, 0)
  423. handle->queue.dev = dev->v4l2_dev->dev;
  424. #endif
  425. rc = vb2_queue_init(&handle->queue);
  426. if (rc) {
  427. pr_err("can't init vb queue\n");
  428. v4l2_fh_del(&handle->vfh);
  429. v4l2_fh_exit(&handle->vfh);
  430. kfree(handle);
  431. return rc;
  432. }
  433. mutex_init(&handle->event_mutex);
  434. mutex_init(&handle->buffer_mutex);
  435. init_completion(&handle->wait);
  436. #ifdef CONFIG_VIDEOBUF2_DMA_CONTIG
  437. INIT_LIST_HEAD(&handle->extdmaqueue);
  438. #endif
  439. handle->event_buf.va = kmalloc(VIV_EVENT_BUF_SIZE, GFP_KERNEL);
  440. handle->event_buf.pa = __pa(handle->event_buf.va);
  441. spin_lock_irqsave(&file_list_lock[handle->vdev->id], flags);
  442. list_add_tail(&handle->entry, &file_list_head[handle->vdev->id]);
  443. spin_unlock_irqrestore(&file_list_lock[handle->vdev->id], flags);
  444. return 0;
  445. }
  446. static int video_close(struct file *file)
  447. {
  448. struct viv_video_file *handle = priv_to_handle(file->private_data);
  449. struct vb2_buffer *vb;
  450. spinlock_t *lock;
  451. unsigned long flags;
  452. pr_debug("enter %s\n", __func__);
  453. if (handle) {
  454. handle->req = false;
  455. if (handle->streamid >= 0 && handle->state == 2) {
  456. set_stream(handle->vdev, 0);
  457. viv_post_simple_event(VIV_VIDEO_EVENT_STOP_STREAM,
  458. handle->streamid, &handle->vfh,
  459. false);
  460. handle->state = 1;
  461. }
  462. if (handle->streamid >= 0 && handle->state == 1) {
  463. viv_post_simple_event(VIV_VIDEO_EVENT_DEL_STREAM,
  464. handle->streamid, &handle->vfh,
  465. true);
  466. handle->vdev->frame_flag = false;
  467. }
  468. if (handle->state > 0)
  469. handle->vdev->active = 0;
  470. handle->state = -1;
  471. handle->streamid = 0;
  472. lock = &file_list_lock[handle->vdev->id];
  473. spin_lock_irqsave(lock, flags);
  474. list_del(&handle->entry);
  475. spin_unlock_irqrestore(lock, flags);
  476. list_for_each_entry(vb, &handle->queue.queued_list,
  477. queued_entry) {
  478. if (vb->state == VB2_BUF_STATE_ACTIVE)
  479. vb2_buffer_done(vb, VB2_BUF_STATE_ERROR);
  480. }
  481. v4l2_fh_del(&handle->vfh);
  482. v4l2_fh_exit(&handle->vfh);
  483. #ifdef CONFIG_VIDEOBUF2_DMA_CONTIG
  484. {
  485. struct ext_dma_buf *edb = NULL;
  486. while (!list_empty(&handle->extdmaqueue)) {
  487. edb = list_first_entry(&handle->extdmaqueue,
  488. struct ext_dma_buf, entry);
  489. if (edb) {
  490. dma_free_attrs(handle->queue.dev,
  491. edb->size, edb->vaddr,
  492. edb->addr,
  493. DMA_ATTR_WRITE_COMBINE);
  494. list_del(&edb->entry);
  495. kfree(edb);
  496. }
  497. }
  498. }
  499. #endif
  500. vb2_queue_release(&handle->queue);
  501. mutex_destroy(&handle->event_mutex);
  502. mutex_destroy(&handle->buffer_mutex);
  503. kfree(handle->event_buf.va);
  504. kfree(handle);
  505. }
  506. return 0;
  507. }
  508. static int subscribe_event(struct v4l2_fh *fh,
  509. const struct v4l2_event_subscription *sub)
  510. {
  511. int ret;
  512. unsigned long flags;
  513. struct viv_video_file *handle = priv_to_handle(fh);
  514. struct viv_video_device *vdev;
  515. if (!handle || !sub)
  516. return -EINVAL;
  517. if (unlikely(sub->type != VIV_VIDEO_EVENT_TYPE))
  518. return v4l2_ctrl_subscribe_event(fh, sub);
  519. ret = v4l2_event_subscribe(fh, sub, 10, 0);
  520. vdev = handle->vdev;
  521. if (!ret && vdev && sub->id == VIV_VIDEO_EVENT_GET_CAPS_SUPPORTS) {
  522. spin_lock_irqsave(&file_list_lock[vdev->id], flags);
  523. vdev->subscribed_cnt++;
  524. spin_unlock_irqrestore(&file_list_lock[vdev->id], flags);
  525. complete_all(&vdev->subscribed_wait);
  526. }
  527. return ret;
  528. }
  529. static int unsubscribe_event(struct v4l2_fh *fh,
  530. const struct v4l2_event_subscription *sub)
  531. {
  532. struct viv_video_file *handle = priv_to_handle(fh);
  533. struct viv_video_file *ph;
  534. struct viv_video_device *vdev;
  535. spinlock_t *lock;
  536. unsigned long flags;
  537. pr_debug("enter %s\n", __func__);
  538. if (!handle || !handle->vdev || !sub)
  539. return 0;
  540. if (unlikely(sub->type != VIV_VIDEO_EVENT_TYPE))
  541. return v4l2_event_unsubscribe(fh, sub);
  542. vdev = handle->vdev;
  543. lock = &file_list_lock[vdev->id];
  544. spin_lock_irqsave(lock, flags);
  545. list_for_each_entry(ph, &file_list_head[vdev->id], entry) {
  546. if (ph == handle && handle->streamid < 0) {
  547. if (sub->id == VIV_VIDEO_EVENT_GET_CAPS_SUPPORTS) {
  548. vdev->subscribed_cnt--;
  549. if (vdev->subscribed_cnt <= 0) {
  550. reinit_completion(
  551. &vdev->subscribed_wait);
  552. vdev->subscribed_cnt = 0;
  553. }
  554. }
  555. spin_unlock_irqrestore(lock, flags);
  556. return v4l2_event_unsubscribe(fh, sub);
  557. }
  558. }
  559. spin_unlock_irqrestore(lock, flags);
  560. return 0;
  561. }
  562. #ifndef ENABLE_IRQ
  563. static void viv_buffer_done(struct viv_video_file *handle, u64 addr)
  564. {
  565. struct vb2_buffer *vb;
  566. mutex_lock(&handle->buffer_mutex);
  567. list_for_each_entry(vb, &handle->queue.queued_list, queued_entry) {
  568. if (!vb)
  569. continue;
  570. #ifdef CONFIG_VIDEOBUF2_DMA_CONTIG
  571. if (vb2_dma_contig_plane_dma_addr(vb, DEF_PLANE_NO) == addr) {
  572. vb->planes[DEF_PLANE_NO].bytesused =
  573. handle->vdev->fmt.fmt.pix.sizeimage;
  574. #endif
  575. #if LINUX_VERSION_CODE > KERNEL_VERSION(5, 0, 0)
  576. vb->timestamp = ktime_get_ns();
  577. #endif
  578. vb2_buffer_done(vb, VB2_BUF_STATE_DONE);
  579. mutex_unlock(&handle->buffer_mutex);
  580. return;
  581. }
  582. }
  583. mutex_unlock(&handle->buffer_mutex);
  584. }
  585. #endif
  586. static int set_caps_mode_event(struct file *file)
  587. {
  588. struct viv_video_device *dev = video_drvdata(file);
  589. struct viv_video_file *handle = priv_to_handle(file->private_data);
  590. struct v4l2_event event;
  591. struct viv_video_event *v_event;
  592. v_event = (struct viv_video_event *)&event.u.data[0];
  593. v_event->stream_id = 0;
  594. v_event->file = &handle->vfh;
  595. v_event->sync = true;
  596. v_event->buf_index = dev->id;
  597. event.type = VIV_VIDEO_EVENT_TYPE;
  598. event.id = VIV_VIDEO_EVENT_SET_CAPSMODE;
  599. return viv_post_event(&event, &handle->vfh, true);
  600. }
  601. static int get_caps_suppots_event(struct file *file)
  602. {
  603. struct viv_video_device *dev = video_drvdata(file);
  604. struct viv_video_file *handle = priv_to_handle(file->private_data);
  605. struct v4l2_event event;
  606. struct viv_video_event *v_event;
  607. v_event = (struct viv_video_event *)&event.u.data[0];
  608. v_event->stream_id = 0;
  609. v_event->file = &handle->vfh;
  610. v_event->sync = true;
  611. v_event->buf_index = dev->id;
  612. event.type = VIV_VIDEO_EVENT_TYPE;
  613. event.id = VIV_VIDEO_EVENT_GET_CAPS_SUPPORTS;
  614. if (dev->subscribed_cnt == 0 &&
  615. wait_for_completion_timeout(&dev->subscribed_wait,
  616. msecs_to_jiffies(VIV_VIDEO_EVENT_TIMOUT_MS)) == 0)
  617. return -ETIMEDOUT;
  618. return viv_post_event(&event, &handle->vfh, true);
  619. }
  620. #ifdef ENABLE_IRQ
  621. static struct media_entity *viv_find_entity(struct viv_video_device *dev,
  622. const char *name)
  623. {
  624. struct v4l2_subdev *sd = NULL;
  625. int i;
  626. for (i = 0; i < dev->sdcount; ++i) {
  627. if (!strncmp(dev->subdevs[i]->name, name, strlen(name))) {
  628. sd = dev->subdevs[i];
  629. break;
  630. }
  631. }
  632. return sd ? &sd->entity : NULL;
  633. }
  634. static int viv_create_link(struct media_entity *source, u16 source_pad,
  635. struct media_entity *sink, u16 sink_pad)
  636. {
  637. int rc = 0;
  638. u32 flags = MEDIA_LNK_FL_ENABLED;
  639. if (!source || !sink)
  640. return -EINVAL;
  641. rc = media_create_pad_link(source, source_pad, sink, sink_pad, flags);
  642. if (rc)
  643. goto end;
  644. rc = media_entity_call(sink, link_setup, &sink->pads[sink_pad],
  645. &source->pads[source_pad], flags);
  646. if (rc)
  647. goto end;
  648. rc = media_entity_call(source, link_setup, &source->pads[source_pad],
  649. &sink->pads[sink_pad], flags);
  650. if (rc)
  651. goto end;
  652. end:
  653. return rc;
  654. }
  655. static int viv_create_default_links(struct viv_video_device *dev)
  656. {
  657. struct media_entity *source, *sink;
  658. source = viv_find_entity(dev, ISP_DEVICE_NAME);
  659. sink = &dev->video->entity;
  660. return viv_create_link(source, ISP_PAD_SOURCE, sink, 0);
  661. }
  662. static int viv_config_dwe(struct viv_video_file *handle, bool enable)
  663. {
  664. struct viv_video_device *vdev = handle->vdev;
  665. int rc;
  666. struct media_entity *source, *sink;
  667. if (!vdev || !vdev->sdcount)
  668. return -EINVAL;
  669. if (vdev->dweEnabled == enable)
  670. return 0;
  671. source = viv_find_entity(vdev, DWE_DEVICE_NAME);
  672. if (!source)
  673. return -EINVAL;
  674. sink = &vdev->video->entity;
  675. media_entity_remove_links(source);
  676. media_entity_remove_links(sink);
  677. if (!enable) {
  678. source = viv_find_entity(vdev, ISP_DEVICE_NAME);
  679. media_entity_remove_links(source);
  680. rc = viv_create_link(source, ISP_PAD_SOURCE, sink, 0);
  681. if (!rc)
  682. vdev->dweEnabled = false;
  683. return rc;
  684. }
  685. rc = viv_create_link(source, DWE_PAD_SOURCE, sink, 0);
  686. if (rc)
  687. goto end;
  688. if (vdev->sdcount > 1) {
  689. source = viv_find_entity(vdev, ISP_DEVICE_NAME);
  690. sink = viv_find_entity(vdev, DWE_DEVICE_NAME);
  691. media_entity_remove_links(source);
  692. if (viv_create_link(source, ISP_PAD_SOURCE, sink, DWE_PAD_SINK))
  693. pr_err("failed to create link between isp and dwe!\n");
  694. }
  695. vdev->dweEnabled = true;
  696. end:
  697. return rc;
  698. }
  699. #endif
  700. static inline void init_v4l2_fmt(struct v4l2_format *f, unsigned int bpp,
  701. unsigned int depth, unsigned int *bytesperline,
  702. unsigned int *sizeimage)
  703. {
  704. v4l_bound_align_image(&f->fmt.pix.width, 48, 3840, 2,
  705. &f->fmt.pix.height, 32, 2160, 0, 0);
  706. *bytesperline = ALIGN_UP(f->fmt.pix.width * bpp, 16);
  707. *sizeimage = f->fmt.pix.height * ALIGN_UP(f->fmt.pix.width * depth / 8, 16);
  708. }
  709. static int viv_set_modeinfo(struct viv_video_file *handle,
  710. struct vvcam_constant_modeinfo *modeinfo)
  711. {
  712. struct viv_video_device *vdev = handle->vdev;
  713. struct viv_video_fmt fmt;
  714. struct viv_video_fmt *pfmt = NULL;
  715. int i;
  716. if (modeinfo->w == 0 || modeinfo->h == 0 || modeinfo->fps == 0) {
  717. vdev->modeinfocount = 0;
  718. memset(vdev->modeinfo, 0, sizeof(vdev->modeinfo));
  719. memset(&vdev->crop, 0, sizeof(struct v4l2_rect));
  720. memset(&vdev->compose, 0, sizeof(struct v4l2_rect));
  721. return -EINVAL;
  722. }
  723. for (i = 0; i < vdev->modeinfocount; ++i) {
  724. if (vdev->modeinfo[i].index == modeinfo->index) {
  725. pr_debug("sensor mode info already configured!\n");
  726. return 0;
  727. }
  728. }
  729. if (vdev->modeinfocount < ARRAY_SIZE(vdev->modeinfo)) {
  730. memcpy(&vdev->modeinfo[vdev->modeinfocount],
  731. modeinfo, sizeof(*modeinfo));
  732. vdev->modeinfocount++;
  733. }
  734. if (!bayer_pattern_to_format(modeinfo->brpat, modeinfo->bitw, &fmt) &&
  735. vdev->formatscount < ARRAY_SIZE(vdev->formats)) {
  736. for (i = 0; i < vdev->formatscount; ++i)
  737. if (vdev->formats[i].fourcc == fmt.fourcc)
  738. break;
  739. if (i == vdev->formatscount) {
  740. memcpy(&vdev->formats[vdev->formatscount],
  741. &fmt, sizeof(fmt));
  742. vdev->formatscount++;
  743. }
  744. }
  745. if (vdev->fmt.fmt.pix.width == 0 || vdev->fmt.fmt.pix.height == 0) {
  746. if (vdev->formats[0].fourcc == 0 || vdev->formats[0].bpp == 0 ||
  747. vdev->formats[0].depth == 0 ||
  748. vdev->modeinfo[0].w == 0 || vdev->modeinfo[0].h == 0 ||
  749. vdev->modeinfo[0].fps == 0)
  750. pr_err("invalid default format!\n");
  751. if (vdev->fmt.fmt.pix.pixelformat == 0) {
  752. vdev->fmt.fmt.pix.pixelformat = vdev->formats[0].fourcc;
  753. }
  754. for (i = 0; i < vdev->formatscount; ++i) {
  755. if (vdev->fmt.fmt.pix.pixelformat == vdev->formats[i].fourcc) {
  756. pfmt = &vdev->formats[i];
  757. break;
  758. }
  759. }
  760. if (pfmt == NULL)
  761. pfmt = &vdev->formats[0];
  762. init_v4l2_fmt(&vdev->fmt, pfmt->bpp, pfmt->depth,
  763. &vdev->fmt.fmt.pix.bytesperline,
  764. &vdev->fmt.fmt.pix.sizeimage);
  765. vdev->timeperframe.numerator = 1;
  766. vdev->timeperframe.denominator = vdev->modeinfo[0].fps;
  767. }
  768. if (vdev->crop.width == 0 || vdev->crop.height == 0) {
  769. vdev->crop.width = vdev->modeinfo[0].w;
  770. vdev->crop.height = vdev->modeinfo[0].h;
  771. }
  772. if (vdev->compose.width == 0 || vdev->compose.height == 0) {
  773. vdev->compose.width = vdev->modeinfo[0].w;
  774. vdev->compose.height = vdev->modeinfo[0].h;
  775. }
  776. return 0;
  777. }
  778. static long private_ioctl(struct file *file, void *fh,
  779. bool valid_prio, unsigned int cmd, void *arg)
  780. {
  781. struct viv_video_file *handle;
  782. struct viv_video_file *ph;
  783. struct viv_video_event *v_event;
  784. struct ext_buf_info *ext_buf;
  785. struct viv_control_event *control_event;
  786. struct viv_caps_supports *pcaps_supports;
  787. struct viv_video_device *dev = video_drvdata(file);
  788. unsigned long flags;
  789. int rc = 0;
  790. struct reserved_mem *rmem;
  791. if (!file || !fh)
  792. return -EINVAL;
  793. handle = priv_to_handle(file->private_data);
  794. if (!handle || handle->state == -1) {
  795. pr_err("call ioctl after file closed\n");
  796. return -EINVAL;
  797. }
  798. switch (cmd) {
  799. case VIV_VIDIOC_EVENT_COMPLETE:
  800. v_event = (struct viv_video_event *)arg;
  801. if (v_event->file) {
  802. handle = priv_to_handle(v_event->file);
  803. spin_lock_irqsave(
  804. &file_list_lock[dev->id], flags);
  805. list_for_each_entry(ph,
  806. &file_list_head[dev->id], entry) {
  807. if (ph == handle) {
  808. complete(&handle->wait);
  809. break;
  810. }
  811. }
  812. spin_unlock_irqrestore(
  813. &file_list_lock[dev->id], flags);
  814. } else {
  815. complete(&dev->ctrls.wait);
  816. }
  817. break;
  818. #ifndef ENABLE_IRQ
  819. case VIV_VIDIOC_BUFDONE: {
  820. /* pr_debug("priv ioctl VIV_VIDIOC_BUFDONE\n"); */
  821. struct v4l2_user_buffer *user_buffer =
  822. (struct v4l2_user_buffer *)arg;
  823. if (!user_buffer->file)
  824. break;
  825. handle = priv_to_handle(user_buffer->file);
  826. if (!handle || handle->state != 2)
  827. break;
  828. /* handle the stream closed unexpected. */
  829. spin_lock_irqsave(&file_list_lock[dev->id], flags);
  830. list_for_each_entry(ph, &file_list_head[dev->id], entry) {
  831. if (ph == handle) {
  832. viv_buffer_done(handle, user_buffer->addr);
  833. break;
  834. }
  835. }
  836. spin_unlock_irqrestore(&file_list_lock[dev->id], flags);
  837. break;
  838. }
  839. #endif
  840. case VIV_VIDIOC_S_STREAMID:
  841. pr_debug("priv ioctl VIV_VIDIOC_S_STREAMID\n");
  842. handle->streamid = *((int *)arg);
  843. break;
  844. #ifdef ENABLE_IRQ
  845. case VIV_VIDIOC_S_DWECFG:
  846. viv_config_dwe(handle, !!*((int *)arg));
  847. break;
  848. case VIV_VIDIOC_G_DWECFG:
  849. *((int *)arg) = handle->vdev->dweEnabled ? 1 : 0;
  850. break;
  851. #endif
  852. case VIV_VIDIOC_BUFFER_ALLOC: {
  853. struct ext_buf_info *ext_buf = (struct ext_buf_info *)arg;
  854. #ifdef CONFIG_VIDEOBUF2_DMA_CONTIG
  855. struct ext_dma_buf *edb = kzalloc(sizeof(*edb), GFP_KERNEL);
  856. pr_debug("priv ioctl VIV_VIDIOC_BUFFER_ALLOC\n");
  857. if (!edb) {
  858. rc = -ENOMEM;
  859. break;
  860. }
  861. edb->vaddr = dma_alloc_attrs(handle->queue.dev,
  862. ext_buf->size, &ext_buf->addr,
  863. GFP_KERNEL, DMA_ATTR_WRITE_COMBINE);
  864. if (!edb->vaddr) {
  865. pr_err("failed to alloc dma buffer!\n");
  866. rc = -ENOMEM;
  867. } else {
  868. edb->addr = ext_buf->addr;
  869. edb->size = ext_buf->size;
  870. list_add_tail(&edb->entry, &handle->extdmaqueue);
  871. }
  872. #endif
  873. break;
  874. }
  875. case VIV_VIDIOC_BUFFER_FREE: {
  876. struct ext_buf_info *ext_buf = (struct ext_buf_info *)arg;
  877. #ifdef CONFIG_VIDEOBUF2_DMA_CONTIG
  878. struct ext_dma_buf *b, *edb = NULL;
  879. pr_debug("priv ioctl VIV_VIDIOC_BUFFER_FREE\n");
  880. list_for_each_entry(b, &handle->extdmaqueue, entry) {
  881. if (b->addr == ext_buf->addr) {
  882. edb = b;
  883. break;
  884. }
  885. }
  886. if (edb) {
  887. dma_free_attrs(handle->queue.dev, edb->size,
  888. edb->vaddr, edb->addr,
  889. DMA_ATTR_WRITE_COMBINE);
  890. list_del(&edb->entry);
  891. kfree(edb);
  892. }
  893. #endif
  894. break;
  895. }
  896. case VIV_VIDIOC_CONTROL_EVENT:
  897. pr_debug("priv ioctl VIV_VIDIOC_CONTROL_EVENT\n");
  898. control_event = (struct viv_control_event *)arg;
  899. rc = viv_post_control_event(handle->streamid, &handle->vfh,
  900. control_event);
  901. break;
  902. case VIV_VIDIOC_QUERY_EXTMEM:
  903. pr_debug("priv ioctl VIV_VIDIOC_QUERY_EXTMEM\n");
  904. ext_buf = (struct ext_buf_info *)arg;
  905. rmem = (struct reserved_mem *)dev->rmem;
  906. if (!rmem) {
  907. ext_buf->addr = 0;
  908. ext_buf->size = 0;
  909. } else {
  910. ext_buf->addr = rmem->base;
  911. ext_buf->size = rmem->size;
  912. }
  913. break;
  914. case VIV_VIDIOC_S_MODEINFO:
  915. rc = viv_set_modeinfo(handle, arg);
  916. break;
  917. case VIV_VIDIOC_S_CAPS_MODE:
  918. memcpy(&(dev->caps_mode), arg, sizeof(dev->caps_mode));
  919. rc = set_caps_mode_event(file);
  920. if (rc == 0)
  921. rc = dev->event_result;
  922. break;
  923. case VIV_VIDIOC_G_CAPS_MODE:
  924. memcpy(arg, &(dev->caps_mode), sizeof(dev->caps_mode));
  925. break;
  926. case VIV_VIDIOC_EVENT_RESULT:
  927. dev->event_result = *(int *)arg;
  928. break;
  929. case VIV_VIDIOC_GET_CAPS_SUPPORTS:{
  930. pcaps_supports = (struct viv_caps_supports *)arg;
  931. rc = get_caps_suppots_event(file);
  932. memcpy(pcaps_supports, &(dev->caps_supports),
  933. sizeof(dev->caps_supports));
  934. break;
  935. }
  936. case VIV_VIDIOC_SET_CAPS_SUPPORTS:
  937. pcaps_supports = (struct viv_caps_supports *)arg;
  938. memcpy(&(dev->caps_supports), arg, sizeof(dev->caps_supports));
  939. break;
  940. }
  941. return rc;
  942. }
  943. static int video_querycap(struct file *file, void *fh,
  944. struct v4l2_capability *cap)
  945. {
  946. struct viv_video_device *dev = video_drvdata(file);
  947. pr_debug("enter %s\n", __func__);
  948. strcpy(cap->driver, "viv_v4l2_device");
  949. strcpy(cap->card, "VIV");
  950. cap->bus_info[0] = 0;
  951. if (dev)
  952. snprintf((char *)cap->bus_info, sizeof(cap->bus_info),
  953. "platform:viv%d", dev->id);
  954. cap->capabilities = V4L2_CAP_VIDEO_CAPTURE |
  955. V4L2_CAP_STREAMING | V4L2_CAP_DEVICE_CAPS;
  956. cap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
  957. return 0;
  958. }
  959. static int vidioc_enum_fmt_vid_cap(struct file *file, void *priv,
  960. struct v4l2_fmtdesc *f)
  961. {
  962. struct viv_video_device *dev = video_drvdata(file);
  963. if (f->index < dev->formatscount) {
  964. f->pixelformat = dev->formats[f->index].fourcc;
  965. return 0;
  966. }
  967. return -EINVAL;
  968. }
  969. static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
  970. struct v4l2_format *f)
  971. {
  972. struct viv_video_file *handle = priv_to_handle(file->private_data);
  973. if (f->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
  974. return -EINVAL;
  975. *f = handle->vdev->fmt;
  976. return 0;
  977. }
  978. static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
  979. struct v4l2_format *f)
  980. {
  981. struct viv_video_device *dev = video_drvdata(file);
  982. struct viv_video_fmt *format = NULL;
  983. int bytesperline, sizeimage;
  984. int i;
  985. pr_debug("enter %s\n", __func__);
  986. if (f->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
  987. return -EINVAL;
  988. f->fmt.pix.width = ALIGN_UP(f->fmt.pix.width, 16);
  989. f->fmt.pix.height = ALIGN_UP(f->fmt.pix.height, 8);
  990. for (i = 0; i < dev->formatscount; ++i) {
  991. if (dev->formats[i].fourcc == f->fmt.pix.pixelformat) {
  992. format = &dev->formats[i];
  993. break;
  994. }
  995. }
  996. if (format == NULL)
  997. return -EINVAL;
  998. f->fmt.pix.field = dev->fmt.fmt.pix.field;
  999. f->fmt.pix.colorspace = dev->fmt.fmt.pix.colorspace;
  1000. init_v4l2_fmt(f, format->bpp, format->depth, &bytesperline, &sizeimage);
  1001. if (f->fmt.pix.bytesperline < bytesperline)
  1002. f->fmt.pix.bytesperline = bytesperline;
  1003. if (f->fmt.pix.sizeimage < sizeimage)
  1004. f->fmt.pix.sizeimage = sizeimage;
  1005. return 0;
  1006. }
  1007. static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
  1008. struct v4l2_format *f)
  1009. {
  1010. struct viv_video_file *handle = priv_to_handle(file->private_data);
  1011. struct viv_video_device *vdev = handle->vdev;
  1012. int ret;
  1013. struct v4l2_event event;
  1014. struct viv_video_event *v_event;
  1015. struct viv_rect *rect = (struct viv_rect *)handle->event_buf.va;
  1016. pr_debug("enter %s\n", __func__);
  1017. if (f->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
  1018. return -EINVAL;
  1019. ret = vidioc_try_fmt_vid_cap(file, priv, f);
  1020. if (ret < 0)
  1021. return -EINVAL;
  1022. handle->vdev->fmt = *f;
  1023. if (vdev->crop.top == 0 &&
  1024. vdev->crop.left == 0 &&
  1025. vdev->crop.width == vdev->compose.width &&
  1026. vdev->crop.height == vdev->compose.height) {
  1027. vdev->compose.width = f->fmt.pix.width;
  1028. vdev->compose.height = f->fmt.pix.height;
  1029. vdev->crop.width = vdev->compose.width;
  1030. vdev->crop.height = vdev->compose.height;
  1031. rect->left = 0;
  1032. rect->top = 0;
  1033. rect->width = vdev->compose.width;
  1034. rect->height = vdev->compose.height;
  1035. v_event = (struct viv_video_event *)&event.u.data[0];
  1036. v_event->stream_id = handle->streamid;
  1037. v_event->file = &(handle->vfh);
  1038. v_event->sync = true;
  1039. v_event->addr = handle->event_buf.pa;
  1040. event.type = VIV_VIDEO_EVENT_TYPE;
  1041. event.id = VIV_VIDEO_EVENT_SET_COMPOSE;
  1042. viv_post_event(&event, &handle->vfh, true);
  1043. rect->left = 0;
  1044. rect->top = 0;
  1045. rect->width = 0;
  1046. rect->height = 0;
  1047. v_event = (struct viv_video_event *)&event.u.data[0];
  1048. v_event->stream_id = handle->streamid;
  1049. v_event->file = &(handle->vfh);
  1050. v_event->sync = true;
  1051. v_event->addr = handle->event_buf.pa;
  1052. event.type = VIV_VIDEO_EVENT_TYPE;
  1053. event.id = VIV_VIDEO_EVENT_SET_CROP;
  1054. viv_post_event(&event, &handle->vfh, true);
  1055. }
  1056. return ret;
  1057. }
  1058. static int vidioc_reqbufs(struct file *file, void *priv,
  1059. struct v4l2_requestbuffers *p)
  1060. {
  1061. struct viv_video_file *handle = priv_to_handle(file->private_data);
  1062. struct viv_video_file *ph;
  1063. struct viv_video_device *vdev = handle->vdev;
  1064. struct v4l2_event event;
  1065. struct viv_video_event *v_event;
  1066. unsigned long flags;
  1067. int ret = 0;
  1068. pr_debug("enter %s %d %d\n", __func__, p->count, p->memory);
  1069. if (p->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
  1070. return -EINVAL;
  1071. spin_lock_irqsave(&file_list_lock[vdev->id], flags);
  1072. list_for_each_entry(ph, &file_list_head[vdev->id], entry) {
  1073. if (ph->streamid == handle->streamid &&
  1074. ph != handle && ph->req) {
  1075. pr_err("stream is busy %d\n", ph->streamid);
  1076. spin_unlock_irqrestore(&file_list_lock[vdev->id],
  1077. flags);
  1078. return -EBUSY;
  1079. }
  1080. }
  1081. spin_unlock_irqrestore(&file_list_lock[vdev->id], flags);
  1082. if (p->count == 0)
  1083. handle->req = false;
  1084. else
  1085. handle->req = true;
  1086. mutex_lock(&handle->buffer_mutex);
  1087. ret = vb2_reqbufs(&handle->queue, p);
  1088. mutex_unlock(&handle->buffer_mutex);
  1089. if (p->count == 0) {
  1090. memset(p, 0, sizeof(*p));
  1091. return ret;
  1092. }
  1093. if (ret < 0)
  1094. return ret;
  1095. if (handle->streamid < 0 || handle->state > 0)
  1096. return ret;
  1097. handle->state = 1;
  1098. ret =
  1099. viv_post_simple_event(VIV_VIDEO_EVENT_NEW_STREAM, handle->streamid,
  1100. &handle->vfh, true);
  1101. if (ret)
  1102. return ret;
  1103. v_event = (struct viv_video_event *)&event.u.data[0];
  1104. v_event->stream_id = handle->streamid;
  1105. v_event->file = &handle->vfh;
  1106. v_event->addr = handle->vdev->fmt.fmt.pix.width;
  1107. v_event->response = handle->vdev->fmt.fmt.pix.height;
  1108. v_event->buf_index = handle->vdev->fmt.fmt.pix.pixelformat;
  1109. v_event->sync = true;
  1110. event.type = VIV_VIDEO_EVENT_TYPE;
  1111. event.id = VIV_VIDEO_EVENT_SET_FMT;
  1112. ret = viv_post_event(&event, &handle->vfh, true);
  1113. return ret;
  1114. }
  1115. static int vidioc_querybuf(struct file *file, void *priv, struct v4l2_buffer *p)
  1116. {
  1117. struct viv_video_file *handle = priv_to_handle(file->private_data);
  1118. int rc = 0;
  1119. pr_debug("enter %s\n", __func__);
  1120. if (p->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
  1121. return -EINVAL;
  1122. mutex_lock(&handle->buffer_mutex);
  1123. rc = vb2_querybuf(&handle->queue, p);
  1124. mutex_unlock(&handle->buffer_mutex);
  1125. return rc;
  1126. }
  1127. static int vidioc_qbuf(struct file *file, void *priv, struct v4l2_buffer *p)
  1128. {
  1129. struct viv_video_file *handle = priv_to_handle(file->private_data);
  1130. int rc = 0;
  1131. if (p->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
  1132. return -EINVAL;
  1133. mutex_lock(&handle->buffer_mutex);
  1134. #if LINUX_VERSION_CODE > KERNEL_VERSION(5, 0, 0)
  1135. rc = vb2_qbuf(&handle->queue, NULL, p);
  1136. #else
  1137. rc = vb2_qbuf(&handle->queue, p);
  1138. #endif
  1139. mutex_unlock(&handle->buffer_mutex);
  1140. return rc;
  1141. }
  1142. static int vidioc_dqbuf(struct file *file, void *priv, struct v4l2_buffer *p)
  1143. {
  1144. struct viv_video_file *handle = priv_to_handle(file->private_data);
  1145. int rc = 0;
  1146. rc = vb2_dqbuf(&handle->queue, p, file->f_flags & O_NONBLOCK);
  1147. p->field = V4L2_FIELD_NONE;
  1148. p->sequence = handle->sequence++;
  1149. return rc;
  1150. }
  1151. static int vidioc_expbuf(struct file *file, void *priv,
  1152. struct v4l2_exportbuffer *p)
  1153. {
  1154. struct viv_video_file *handle = priv_to_handle(file->private_data);
  1155. pr_debug("enter %s\n", __func__);
  1156. return vb2_expbuf(&handle->queue, p);
  1157. }
  1158. static int vidioc_streamon(struct file *file, void *priv, enum v4l2_buf_type i)
  1159. {
  1160. struct viv_video_file *handle = priv_to_handle(file->private_data);
  1161. pr_debug("enter %s\n", __func__);
  1162. return vb2_streamon(&handle->queue, i);
  1163. }
  1164. static int vidioc_streamoff(struct file *file, void *priv, enum v4l2_buf_type i)
  1165. {
  1166. struct viv_video_file *handle = priv_to_handle(file->private_data);
  1167. int rc;
  1168. pr_debug("enter %s\n", __func__);
  1169. mutex_lock(&handle->buffer_mutex);
  1170. rc = vb2_streamoff(&handle->queue, i);
  1171. mutex_unlock(&handle->buffer_mutex);
  1172. return rc;
  1173. }
  1174. static int vidioc_enum_input(struct file *filep, void *fh,
  1175. struct v4l2_input *input)
  1176. {
  1177. if (input->index > 0)
  1178. return -EINVAL;
  1179. strlcpy(input->name, "camera", sizeof(input->name));
  1180. input->type = V4L2_INPUT_TYPE_CAMERA;
  1181. return 0;
  1182. }
  1183. static int vidioc_g_input(struct file *filep, void *fh, unsigned int *input)
  1184. {
  1185. *input = 0;
  1186. return 0;
  1187. }
  1188. static int vidioc_s_input(struct file *filep, void *fh, unsigned int input)
  1189. {
  1190. return input == 0 ? 0 : -EINVAL;
  1191. }
  1192. static int vidioc_enum_framesizes(struct file *file, void *priv,
  1193. struct v4l2_frmsizeenum *fsize)
  1194. {
  1195. struct viv_video_device *dev = video_drvdata(file);
  1196. struct viv_video_file *handle = priv_to_handle(file->private_data);
  1197. int i;
  1198. if (dev->modeinfocount == 0) {
  1199. viv_post_simple_event(VIV_VIDEO_EVENT_CREATE_PIPELINE,
  1200. handle->streamid, &handle->vfh, true);
  1201. }
  1202. if (fsize->index >= dev->modeinfocount)
  1203. return -EINVAL;
  1204. for (i = 0; i < dev->formatscount; ++i)
  1205. if (dev->formats[i].fourcc == fsize->pixel_format)
  1206. break;
  1207. if (i == dev->formatscount)
  1208. return -EINVAL;
  1209. fsize->stepwise.min_width = 176;
  1210. fsize->stepwise.max_width = dev->modeinfo[fsize->index].w;
  1211. fsize->stepwise.step_width = 16;
  1212. fsize->stepwise.min_height = 144;
  1213. fsize->stepwise.max_height = dev->modeinfo[fsize->index].h;
  1214. fsize->stepwise.step_height = 8;
  1215. fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE;
  1216. return 0;
  1217. }
  1218. static inline void update_timeperframe(struct file *file)
  1219. {
  1220. struct viv_video_device *dev = video_drvdata(file);
  1221. int i;
  1222. if (dev->timeperframe.numerator == 0 ||
  1223. dev->timeperframe.denominator == 0) {
  1224. dev->timeperframe.numerator = 1;
  1225. dev->timeperframe.denominator = 30;
  1226. for (i = 0; i < dev->modeinfocount; ++i) {
  1227. if (dev->fmt.fmt.pix.width == dev->modeinfo[i].w &&
  1228. dev->fmt.fmt.pix.height == dev->modeinfo[i].h) {
  1229. dev->timeperframe.denominator =
  1230. dev->modeinfo[i].fps;
  1231. break;
  1232. }
  1233. }
  1234. }
  1235. }
  1236. static int vidioc_g_parm(struct file *file, void *fh,
  1237. struct v4l2_streamparm *a)
  1238. {
  1239. struct viv_video_file *handle = priv_to_handle(file->private_data);
  1240. if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
  1241. return -EINVAL;
  1242. update_timeperframe(file);
  1243. memset(&a->parm, 0, sizeof(a->parm));
  1244. a->parm.capture.capability = V4L2_CAP_TIMEPERFRAME;
  1245. a->parm.capture.timeperframe = handle->vdev->timeperframe;
  1246. return 0;
  1247. }
  1248. static int vidioc_s_parm(struct file *file, void *fh,
  1249. struct v4l2_streamparm *a)
  1250. {
  1251. struct viv_video_file *handle = priv_to_handle(file->private_data);
  1252. struct viv_video_device *vdev = handle->vdev;
  1253. struct v4l2_event event;
  1254. struct viv_video_event *v_event;
  1255. if (a->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
  1256. return -EINVAL;
  1257. if (a->parm.capture.timeperframe.numerator == 0 ||
  1258. a->parm.capture.timeperframe.denominator == 0) {
  1259. update_timeperframe(file);
  1260. memset(&a->parm, 0, sizeof(a->parm));
  1261. a->parm.capture.capability = V4L2_CAP_TIMEPERFRAME;
  1262. a->parm.capture.timeperframe = handle->vdev->timeperframe;
  1263. return 0;
  1264. }
  1265. handle->vdev->timeperframe = a->parm.output.timeperframe;
  1266. if (vdev->ctrls.buf_va == NULL)
  1267. return -EINVAL;
  1268. sprintf(vdev->ctrls.buf_va,"{<id>:<s.fps>;<fps>:%d}",handle->vdev->timeperframe.denominator);
  1269. v_event = (struct viv_video_event *)&event.u.data[0];
  1270. v_event->stream_id = 0;
  1271. v_event->file = &handle->vfh;
  1272. v_event->sync = true;
  1273. v_event->addr = vdev->ctrls.buf_pa;
  1274. event.type = VIV_VIDEO_EVENT_TYPE;
  1275. event.id = VIV_VIDEO_EVENT_EXTCTRL;
  1276. viv_post_event(&event, &handle->vfh, true);
  1277. return 0;
  1278. }
  1279. static int vidioc_enum_frameintervals(struct file *filp, void *priv,
  1280. struct v4l2_frmivalenum *fival)
  1281. {
  1282. struct viv_video_device *dev = video_drvdata(filp);
  1283. struct viv_video_file *handle = priv_to_handle(filp->private_data);
  1284. int i;
  1285. if (dev->modeinfocount == 0) {
  1286. viv_post_simple_event(VIV_VIDEO_EVENT_CREATE_PIPELINE,
  1287. handle->streamid, &handle->vfh, true);
  1288. }
  1289. for (i = 0; i < dev->formatscount; ++i)
  1290. if (dev->formats[i].fourcc == fival->pixel_format)
  1291. break;
  1292. if (i == dev->formatscount)
  1293. return -EINVAL;
  1294. if (fival->index >= dev->modeinfo[0].fps)
  1295. return -EINVAL;
  1296. if (fival->width % 16 || fival->height % 8 ||
  1297. fival->width < 176 || fival->height < 144 ||
  1298. fival->width > dev->modeinfo[0].w ||
  1299. fival->height > dev->modeinfo[0].h)
  1300. return -EINVAL;
  1301. fival->discrete.numerator = 1;
  1302. fival->discrete.denominator = dev->modeinfo[0].fps - fival->index;
  1303. fival->type = V4L2_FRMSIZE_TYPE_DISCRETE;
  1304. return 0;
  1305. }
  1306. static int vidioc_g_pixelaspect(struct file *file, void *fh,
  1307. int buf_type, struct v4l2_fract *aspect)
  1308. {
  1309. if (buf_type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
  1310. return -EINVAL;
  1311. pr_debug("%s not implemented\n", __func__);
  1312. return 0;
  1313. }
  1314. static int vidioc_g_selection(struct file *file, void *fh,
  1315. struct v4l2_selection *s)
  1316. {
  1317. struct viv_video_file *handle = priv_to_handle(file->private_data);
  1318. struct viv_video_device *vdev = handle->vdev;
  1319. if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
  1320. return -EINVAL;
  1321. if (vdev->modeinfocount == 0) {
  1322. viv_post_simple_event(VIV_VIDEO_EVENT_CREATE_PIPELINE,
  1323. handle->streamid, &handle->vfh, true);
  1324. }
  1325. switch (s->target) {
  1326. case V4L2_SEL_TGT_CROP_DEFAULT:
  1327. case V4L2_SEL_TGT_CROP_BOUNDS:
  1328. s->r.left = 0;
  1329. s->r.top = 0;
  1330. s->r.width = vdev->compose.width;
  1331. s->r.height = vdev->compose.height;
  1332. break;
  1333. case V4L2_SEL_TGT_CROP:
  1334. s->r = vdev->crop;
  1335. break;
  1336. case V4L2_SEL_TGT_COMPOSE_DEFAULT:
  1337. case V4L2_SEL_TGT_COMPOSE_BOUNDS:
  1338. s->r.left = 0;
  1339. s->r.top = 0;
  1340. s->r.width = 3840;
  1341. s->r.height = 2160;
  1342. break;
  1343. case V4L2_SEL_TGT_COMPOSE:
  1344. s->r = vdev->compose;
  1345. break;
  1346. default:
  1347. return -EINVAL;
  1348. }
  1349. return 0;
  1350. }
  1351. static int vidioc_s_selection(struct file *file, void *fh,
  1352. struct v4l2_selection *s)
  1353. {
  1354. struct viv_video_file *handle = priv_to_handle(file->private_data);
  1355. struct viv_video_device *vdev = handle->vdev;
  1356. struct v4l2_rect *r;
  1357. struct v4l2_event event;
  1358. struct viv_video_event *v_event;
  1359. struct viv_rect *rect = (struct viv_rect *)handle->event_buf.va;
  1360. int rc;
  1361. int crop_flag = 1;
  1362. if (!rect)
  1363. return -ENOMEM;
  1364. if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
  1365. return -EINVAL;
  1366. if (vdev->modeinfocount == 0) {
  1367. viv_post_simple_event(VIV_VIDEO_EVENT_CREATE_PIPELINE,
  1368. handle->streamid, &handle->vfh, true);
  1369. }
  1370. switch (s->target) {
  1371. case V4L2_SEL_TGT_CROP:
  1372. r = &vdev->crop;
  1373. break;
  1374. case V4L2_SEL_TGT_COMPOSE:
  1375. r = &vdev->compose;
  1376. break;
  1377. default:
  1378. return -EINVAL;
  1379. }
  1380. if (s->r.top < 0 || s->r.left < 0)
  1381. return -EINVAL;
  1382. if (s->r.width == 0 || s->r.height == 0 ||
  1383. s->r.width > 3840 || s->r.height > 2160)
  1384. return -EINVAL;
  1385. if (s->target == V4L2_SEL_TGT_CROP) {
  1386. if (s->r.left + s->r.width > vdev->compose.width ||
  1387. s->r.top + s->r.height > vdev->compose.height)
  1388. return -EINVAL;
  1389. }
  1390. if (s->target == V4L2_SEL_TGT_COMPOSE) {
  1391. if (vdev->crop.top == 0 &&
  1392. vdev->crop.left == 0 &&
  1393. vdev->crop.width == vdev->compose.width &&
  1394. vdev->crop.height == vdev->compose.height) {
  1395. crop_flag = 0;
  1396. }
  1397. }
  1398. rect->left = s->r.left;
  1399. rect->top = s->r.top;
  1400. rect->width = s->r.width;
  1401. rect->height = s->r.height;
  1402. v_event = (struct viv_video_event *)&event.u.data[0];
  1403. v_event->stream_id = handle->streamid;
  1404. v_event->file = &(handle->vfh);
  1405. v_event->sync = true;
  1406. v_event->addr = handle->event_buf.pa;
  1407. event.type = VIV_VIDEO_EVENT_TYPE;
  1408. if (s->target == V4L2_SEL_TGT_CROP)
  1409. event.id = VIV_VIDEO_EVENT_SET_CROP;
  1410. else
  1411. event.id = VIV_VIDEO_EVENT_SET_COMPOSE;
  1412. rc = viv_post_event(&event, &handle->vfh, true);
  1413. if (rc == 0) {
  1414. s->r.left = rect->left;
  1415. s->r.top = rect->top;
  1416. s->r.width = rect->width;
  1417. s->r.height = rect->height;
  1418. *r = s->r;
  1419. if (s->target == V4L2_SEL_TGT_COMPOSE && crop_flag == 0) {
  1420. vdev->crop.width = vdev->compose.width;
  1421. vdev->crop.height = vdev->compose.height;
  1422. }
  1423. }
  1424. return rc;
  1425. }
  1426. int viv_gen_g_ctrl(struct v4l2_ctrl *ctrl)
  1427. {
  1428. struct viv_custom_ctrls *cc =
  1429. container_of(ctrl->handler, struct viv_custom_ctrls, handler);
  1430. struct viv_video_device *vdev =
  1431. container_of(cc, struct viv_video_device, ctrls);
  1432. struct v4l2_event event;
  1433. struct viv_video_event *v_event;
  1434. struct v4l2_ctrl_data *p_data;
  1435. struct v4l2_ext_control *p_ctrl;
  1436. pr_debug("%s:ctrl->id=0x%x\n", __func__, ctrl->id);
  1437. memset(&event, 0, sizeof(event));
  1438. v_event = (struct viv_video_event *)&event.u.data[0];
  1439. v_event->file = NULL;
  1440. v_event->sync = true;
  1441. v_event->addr = vdev->ctrls.buf_pa;
  1442. event.type = VIV_VIDEO_EVENT_TYPE;
  1443. event.id = VIV_VIDEO_EVENT_EXTCTRL2;
  1444. if(!video_event_subscribed(vdev->video, &event, RETRY_TIMES_MAX)) {
  1445. pr_err("%s: unsubscribed event id =%d type=0x%08x",
  1446. __func__, event.id, event.type);
  1447. return -EINVAL;
  1448. }
  1449. p_data = (struct v4l2_ctrl_data *)vdev->ctrls.buf_va;
  1450. if (unlikely(!p_data))
  1451. return -ENOMEM;
  1452. memset(p_data, 0, sizeof(*p_data));
  1453. p_data->dir = V4L2_CTRL_GET;
  1454. p_data->ctrls.count = 1;
  1455. p_ctrl = nextof(p_data, struct v4l2_ext_control *);
  1456. p_ctrl->id = ctrl->id;
  1457. reinit_completion(&vdev->ctrls.wait);
  1458. v4l2_event_queue(vdev->video, &event);
  1459. if (wait_for_completion_timeout(&vdev->ctrls.wait,
  1460. msecs_to_jiffies(VIV_VIDEO_EVENT_TIMOUT_MS)) == 0) {
  1461. pr_err("connecting to server timed out (g)!");
  1462. *ctrl->p_new.p_s32 = *ctrl->p_cur.p_s32;
  1463. return 0;
  1464. }
  1465. if (p_ctrl->id == ctrl->id && !p_data->ret) {
  1466. *ctrl->p_new.p_s32 = p_ctrl->value;
  1467. return 0;
  1468. }
  1469. return -EINVAL;
  1470. }
  1471. int viv_gen_s_ctrl(struct v4l2_ctrl *ctrl)
  1472. {
  1473. struct viv_custom_ctrls *cc =
  1474. container_of(ctrl->handler, struct viv_custom_ctrls, handler);
  1475. struct viv_video_device *vdev =
  1476. container_of(cc, struct viv_video_device, ctrls);
  1477. struct v4l2_event event;
  1478. struct viv_video_event *v_event;
  1479. struct v4l2_ctrl_data *p_data;
  1480. struct v4l2_ext_control *p_ctrl;
  1481. pr_debug("%s:ctrl->id=0x%x\n", __func__, ctrl->id);
  1482. memset(&event, 0, sizeof(event));
  1483. v_event = (struct viv_video_event *)&event.u.data[0];
  1484. v_event->file = NULL;
  1485. v_event->sync = true;
  1486. v_event->addr = vdev->ctrls.buf_pa;
  1487. event.type = VIV_VIDEO_EVENT_TYPE;
  1488. event.id = VIV_VIDEO_EVENT_EXTCTRL2;
  1489. if(!video_event_subscribed(vdev->video, &event, RETRY_TIMES_MAX)) {
  1490. pr_err("%s: unsubscribed event id =%d type=0x%08x",
  1491. __func__, event.id, event.type);
  1492. return -EINVAL;
  1493. }
  1494. p_data = (struct v4l2_ctrl_data *)vdev->ctrls.buf_va;
  1495. if (unlikely(!p_data))
  1496. return -ENOMEM;
  1497. memset(p_data, 0, sizeof(*p_data));
  1498. p_data->dir = V4L2_CTRL_SET;
  1499. p_data->ctrls.count = 1;
  1500. p_ctrl = nextof(p_data, struct v4l2_ext_control *);
  1501. p_ctrl->id = ctrl->id;
  1502. p_ctrl->value = *ctrl->p_new.p_s32;
  1503. reinit_completion(&vdev->ctrls.wait);
  1504. v4l2_event_queue(vdev->video, &event);
  1505. if (wait_for_completion_timeout(&vdev->ctrls.wait,
  1506. msecs_to_jiffies(VIV_VIDEO_EVENT_TIMOUT_MS)) == 0) {
  1507. pr_err("connecting to server timed out (s)!");
  1508. return 0;
  1509. }
  1510. if (p_ctrl->id == ctrl->id && !p_data->ret)
  1511. return 0;
  1512. return -EINVAL;
  1513. }
  1514. static const struct v4l2_ioctl_ops video_ioctl_ops = {
  1515. .vidioc_querycap = video_querycap,
  1516. .vidioc_enum_fmt_vid_cap = vidioc_enum_fmt_vid_cap,
  1517. .vidioc_g_fmt_vid_cap = vidioc_g_fmt_vid_cap,
  1518. .vidioc_try_fmt_vid_cap = vidioc_try_fmt_vid_cap,
  1519. .vidioc_s_fmt_vid_cap = vidioc_s_fmt_vid_cap,
  1520. .vidioc_reqbufs = vidioc_reqbufs,
  1521. .vidioc_querybuf = vidioc_querybuf,
  1522. .vidioc_qbuf = vidioc_qbuf,
  1523. .vidioc_expbuf = vidioc_expbuf,
  1524. .vidioc_dqbuf = vidioc_dqbuf,
  1525. .vidioc_streamon = vidioc_streamon,
  1526. .vidioc_streamoff = vidioc_streamoff,
  1527. .vidioc_subscribe_event = subscribe_event,
  1528. .vidioc_unsubscribe_event = unsubscribe_event,
  1529. .vidioc_default = private_ioctl,
  1530. .vidioc_enum_input = vidioc_enum_input,
  1531. .vidioc_g_input = vidioc_g_input,
  1532. .vidioc_s_input = vidioc_s_input,
  1533. .vidioc_enum_framesizes = vidioc_enum_framesizes,
  1534. .vidioc_enum_frameintervals = vidioc_enum_frameintervals,
  1535. .vidioc_g_parm = vidioc_g_parm,
  1536. .vidioc_s_parm = vidioc_s_parm,
  1537. .vidioc_g_pixelaspect = vidioc_g_pixelaspect,
  1538. .vidioc_g_selection = vidioc_g_selection,
  1539. .vidioc_s_selection = vidioc_s_selection,
  1540. };
  1541. /* sys /dev/mem can't map large memory size */
  1542. static int viv_private_mmap(struct file *file, struct vm_area_struct *vma)
  1543. {
  1544. /* Map reserved video memory. */
  1545. if (remap_pfn_range(vma, vma->vm_start, vma->vm_pgoff,
  1546. vma->vm_end - vma->vm_start, vma->vm_page_prot))
  1547. return -EAGAIN;
  1548. return 0;
  1549. }
  1550. int vidioc_mmap(struct file *file, struct vm_area_struct *vma)
  1551. {
  1552. int rc;
  1553. struct viv_video_file *handle = priv_to_handle(file->private_data);
  1554. #ifndef ENABLE_IRQ
  1555. struct viv_video_device *dev = video_drvdata(file);
  1556. struct reserved_mem *rmem = (struct reserved_mem *)dev->rmem;
  1557. unsigned long reserved_base_addr = 0;
  1558. if (!rmem)
  1559. reserved_base_addr = 0;
  1560. else
  1561. reserved_base_addr = rmem->base;
  1562. #endif
  1563. #ifdef ENABLE_IRQ
  1564. if (handle->streamid < 0)
  1565. #else
  1566. if (vma->vm_pgoff >= (reserved_base_addr >> PAGE_SHIFT))
  1567. #endif
  1568. rc = viv_private_mmap(file, vma);
  1569. else
  1570. rc = vb2_mmap(&handle->queue, vma);
  1571. return rc;
  1572. }
  1573. static unsigned int video_poll(struct file *file,
  1574. struct poll_table_struct *wait)
  1575. {
  1576. struct viv_video_file *handle = priv_to_handle(file->private_data);
  1577. int rc = 0;
  1578. if (handle->streamid < 0) {
  1579. poll_wait(file, &handle->vfh.wait, wait);
  1580. if (v4l2_event_pending(&handle->vfh))
  1581. rc = POLLIN | POLLRDNORM;
  1582. } else {
  1583. mutex_lock(&handle->buffer_mutex);
  1584. rc = vb2_poll(&handle->queue, file, wait) |
  1585. v4l2_ctrl_poll(file, wait);
  1586. mutex_unlock(&handle->buffer_mutex);
  1587. }
  1588. return rc;
  1589. }
  1590. static struct v4l2_file_operations video_ops = {
  1591. .owner = THIS_MODULE,
  1592. .open = video_open,
  1593. .release = video_close,
  1594. .poll = video_poll,
  1595. .unlocked_ioctl = video_ioctl2,
  1596. .mmap = vidioc_mmap,
  1597. };
  1598. static void pdev_release(struct device *dev)
  1599. {
  1600. pr_debug("enter %s\n", __func__);
  1601. }
  1602. static struct platform_device viv_pdev = {
  1603. .name = "vvcam-video",
  1604. .dev.release = pdev_release,
  1605. };
  1606. static int viv_s_ctrl(struct v4l2_ctrl *ctrl)
  1607. {
  1608. struct v4l2_event event;
  1609. struct viv_video_event *v_event;
  1610. struct viv_custom_ctrls *cc =
  1611. container_of(ctrl->handler, struct viv_custom_ctrls, handler);
  1612. struct viv_video_device *vdev =
  1613. container_of(cc, struct viv_video_device, ctrls);
  1614. int ret = -1;
  1615. char *szbuf = NULL;
  1616. unsigned long timeout =
  1617. msecs_to_jiffies(VIV_VIDEO_EVENT_TIMOUT_MS);
  1618. switch (ctrl->id) {
  1619. case V4L2_CID_VIV_STRING: {
  1620. ret = 0;
  1621. szbuf = (char *)vdev->ctrls.buf_va;
  1622. if (!ctrl->p_new.p_char || !szbuf)
  1623. return -EINVAL;
  1624. strcpy(szbuf, ctrl->p_new.p_char);
  1625. v_event = (struct viv_video_event *)&event.u.data[0];
  1626. v_event->stream_id = 0;
  1627. v_event->file = NULL;
  1628. v_event->sync = true;
  1629. v_event->addr = vdev->ctrls.buf_pa;
  1630. event.type = VIV_VIDEO_EVENT_TYPE;
  1631. event.id = VIV_VIDEO_EVENT_EXTCTRL;
  1632. if(!video_event_subscribed(vdev->video, &event, RETRY_TIMES_MAX)) {
  1633. pr_err("%s: unsubscribed event id =%d type=0x%08x",
  1634. __func__, event.id, event.type);
  1635. return -EINVAL;
  1636. }
  1637. reinit_completion(&vdev->ctrls.wait);
  1638. v4l2_event_queue(vdev->video, &event);
  1639. if (!wait_for_completion_timeout(&vdev->ctrls.wait, timeout))
  1640. ret = -ETIMEDOUT;
  1641. strcpy(ctrl->p_new.p_char, szbuf);
  1642. break;
  1643. }
  1644. }
  1645. return ret;
  1646. }
  1647. static const struct v4l2_ctrl_ops viv_ctrl_ops = {
  1648. .s_ctrl = viv_s_ctrl,
  1649. };
  1650. static const struct v4l2_ctrl_config viv_ext_ctrl = {
  1651. .ops = &viv_ctrl_ops,
  1652. .id = V4L2_CID_VIV_STRING,
  1653. .name = "viv_ext_ctrl",
  1654. .type = V4L2_CTRL_TYPE_STRING,
  1655. .max = VIV_JSON_BUFFER_SIZE-1,
  1656. .step = 1,
  1657. };
  1658. #ifdef ENABLE_IRQ
  1659. static int viv_notifier_bound(struct v4l2_async_notifier *notifier,
  1660. struct v4l2_subdev *sd, struct v4l2_async_subdev *asd)
  1661. {
  1662. int i;
  1663. struct viv_video_device *dev = container_of(notifier,
  1664. struct viv_video_device, subdev_notifier);
  1665. if (!dev)
  1666. return 0;
  1667. for (i = 0; i < dev->asdcount; ++i) {
  1668. if (dev->asd[i]->match_type == V4L2_ASYNC_MATCH_FWNODE) {
  1669. if (sd->dev && dev->asd[i]->match.fwnode ==
  1670. of_fwnode_handle(sd->dev->of_node)) {
  1671. dev->subdevs[dev->sdcount] = sd;
  1672. dev->sdcount++;
  1673. break;
  1674. }
  1675. } else if (dev->asd[i]->match_type ==
  1676. V4L2_ASYNC_MATCH_DEVNAME) {
  1677. if (sd->dev && !strcmp(dev->asd[i]->match.device_name,
  1678. dev_name(sd->dev))) {
  1679. dev->subdevs[dev->sdcount] = sd;
  1680. dev->sdcount++;
  1681. break;
  1682. }
  1683. }
  1684. }
  1685. return 0;
  1686. }
  1687. static int viv_notifier_complete(struct v4l2_async_notifier *notifier)
  1688. {
  1689. struct viv_video_device *dev = container_of(notifier,
  1690. struct viv_video_device, subdev_notifier);
  1691. int rc;
  1692. if (!dev)
  1693. return 0;
  1694. if (dev->sdcount > 0) {
  1695. mutex_lock(&dev->mdev->graph_mutex);
  1696. rc = viv_create_default_links(dev);
  1697. mutex_unlock(&dev->mdev->graph_mutex);
  1698. if (rc) {
  1699. pr_err("failed to create media links!\n");
  1700. return rc;
  1701. }
  1702. }
  1703. return v4l2_device_register_subdev_nodes(dev->v4l2_dev);
  1704. }
  1705. static const struct v4l2_async_notifier_operations sd_async_notifier_ops = {
  1706. .bound = viv_notifier_bound,
  1707. .complete = viv_notifier_complete,
  1708. };
  1709. static int viv_mdev_link_notify(struct media_link *link, unsigned int flags,
  1710. unsigned int notification)
  1711. {
  1712. return 0;
  1713. }
  1714. static const struct media_device_ops viv_mdev_ops = {
  1715. .link_notify = viv_mdev_link_notify,
  1716. };
  1717. static int viv_link_setup(struct media_entity *entity,
  1718. const struct media_pad *local,
  1719. const struct media_pad *remote, u32 flags)
  1720. {
  1721. return 0;
  1722. }
  1723. static const struct media_entity_operations viv_media_ops = {
  1724. .link_setup = viv_link_setup,
  1725. .link_validate = v4l2_subdev_link_validate,
  1726. };
  1727. static void viv_buf_notify(struct vvbuf_ctx *ctx, struct vb2_dc_buf *buf)
  1728. {
  1729. struct viv_video_file *fh;
  1730. struct viv_video_device *vdev;
  1731. u64 cur_ts, interval;
  1732. u32 fps;
  1733. int i;
  1734. if (!buf || buf->vb.vb2_buf.state != VB2_BUF_STATE_ACTIVE)
  1735. return;
  1736. fh = container_of(buf->vb.vb2_buf.vb2_queue,
  1737. struct viv_video_file, queue);
  1738. vdev = fh->vdev;
  1739. if (!vdev->active)
  1740. return;
  1741. buf->vb.vb2_buf.planes[DEF_PLANE_NO].bytesused =
  1742. vdev->fmt.fmt.pix.sizeimage;
  1743. cur_ts = ktime_get_ns();
  1744. #if LINUX_VERSION_CODE > KERNEL_VERSION(5, 0, 0)
  1745. buf->vb.vb2_buf.timestamp = cur_ts;
  1746. #endif
  1747. vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_DONE);
  1748. /* print fps info for debugging purpose */
  1749. interval = ktime_us_delta(cur_ts,vdev->last_ts);
  1750. for (i = 0; i < VIDEO_NODE_NUM; i++) {
  1751. if (vdev->id == i) {
  1752. if (vdev->duration >= 3 * 1000000/*ms*/) {
  1753. vdev->loop_cnt[i]++;
  1754. if (vdev->loop_cnt[i] >= 10) {
  1755. if (vdev->frame_flag) {
  1756. fps = vdev->frameCnt[i] * 100000000 / vdev->duration;
  1757. pr_info("###### video%d(%d) %d.%02d fps ######\n",
  1758. vdev->video->num, vdev->id,
  1759. fps / 100, fps % 100);
  1760. } else {
  1761. vdev->frame_flag = true;
  1762. }
  1763. vdev->loop_cnt[i] = 0;
  1764. }
  1765. vdev->frameCnt[i] = 0;
  1766. vdev->duration = 0;
  1767. } else if (interval > 0) {
  1768. vdev->frameCnt[i]++;
  1769. vdev->duration += interval;
  1770. }
  1771. }
  1772. }
  1773. vdev->last_ts = cur_ts;
  1774. }
  1775. static const struct vvbuf_ops viv_buf_ops = {
  1776. .notify = viv_buf_notify,
  1777. };
  1778. #endif
  1779. struct dev_node {
  1780. enum v4l2_async_match_type match_type;
  1781. struct device_node *node;
  1782. int id;
  1783. const char *name;
  1784. };
  1785. static const char * const dwe_dev_compat_name[] = {
  1786. "32e30000.dwe", "fsl,fake-imx8mp-dwe.1"};
  1787. static inline int viv_find_compatible_nodes(struct dev_node *nodes, int size)
  1788. {
  1789. static const char * const compat_name[] = {
  1790. ISP_COMPAT_NAME};
  1791. struct device_node *node, *avail;
  1792. int i, rc, id, cnt = 0;
  1793. for (i = 0; i < ARRAY_SIZE(compat_name) && cnt < size; ++i) {
  1794. node = NULL;
  1795. for (; cnt < size;) {
  1796. node = of_find_compatible_node(node,
  1797. NULL, compat_name[i]);
  1798. if (!node)
  1799. break;
  1800. avail = NULL;
  1801. id = -1;
  1802. rc = fwnode_property_read_u32(
  1803. of_fwnode_handle(node), "id", &id);
  1804. if (rc) {
  1805. if (of_device_is_available(node))
  1806. avail = node;
  1807. } else if (of_device_is_available(node))
  1808. avail = node;
  1809. if (avail) {
  1810. nodes[cnt].node = avail;
  1811. nodes[cnt].id = id;
  1812. nodes[cnt].match_type = V4L2_ASYNC_MATCH_FWNODE;
  1813. cnt++;
  1814. nodes[cnt].id = id;
  1815. nodes[cnt].match_type = V4L2_ASYNC_MATCH_DEVNAME;
  1816. nodes[cnt].name = dwe_dev_compat_name[id];
  1817. cnt++;
  1818. }
  1819. }
  1820. }
  1821. return cnt;
  1822. }
  1823. static struct reserved_mem * viv_find_isp_reserve_mem(int dev_id)
  1824. {
  1825. int i,rc;
  1826. int id=0;
  1827. struct device_node *node = NULL;
  1828. struct device_node *mem_node;
  1829. for (i=0; i<VIDEO_NODE_NUM; i++)
  1830. {
  1831. node = of_find_compatible_node(node,
  1832. NULL, ISP_COMPAT_NAME);
  1833. if (!node)
  1834. break;
  1835. rc = fwnode_property_read_u32(
  1836. of_fwnode_handle(node), "id", &id);
  1837. if (rc) {
  1838. pr_err("%s:get fwnode id failed \n",__func__);
  1839. break;
  1840. }
  1841. if (id == dev_id) {
  1842. of_node_put(node);
  1843. mem_node = of_parse_phandle(node, "memory-region", 0);
  1844. if (!mem_node) {
  1845. pr_err("No memory-region found\n");
  1846. return NULL;
  1847. }
  1848. return of_reserved_mem_lookup(mem_node);
  1849. }
  1850. }
  1851. if (node)
  1852. of_node_put(node);
  1853. return NULL;
  1854. }
  1855. static int viv_video_probe(struct platform_device *pdev)
  1856. {
  1857. struct viv_video_device *vdev;
  1858. int rc = 0;
  1859. int i,m, video_id;
  1860. struct dev_node nodes[MAX_SUBDEVS_NUM];
  1861. int nodecount;
  1862. #ifdef ENABLE_IRQ
  1863. int j;
  1864. struct v4l2_async_subdev *asd;
  1865. strscpy(mdev.model, "viv_media", sizeof(mdev.model));
  1866. mdev.ops = &viv_mdev_ops;
  1867. mdev.dev = &pdev->dev;
  1868. media_device_init(&mdev);
  1869. #endif
  1870. memset(nodes, 0, sizeof(nodes));
  1871. nodecount = viv_find_compatible_nodes(nodes, MAX_SUBDEVS_NUM);
  1872. for (i = 0; i < VIDEO_NODE_NUM && i*2 < nodecount; i++) {
  1873. if(nodes[i*2].node) {
  1874. video_id = nodes[i*2].id ;
  1875. if(video_id >= VIDEO_NODE_NUM) {
  1876. pr_err("%s: id %d is too large (id > %d) \n",
  1877. __func__, video_id, VIDEO_NODE_NUM);
  1878. rc = -EINVAL;
  1879. goto probe_end;
  1880. }
  1881. spin_lock_init(&file_list_lock[video_id]);
  1882. INIT_LIST_HEAD(&file_list_head[video_id]);
  1883. vvdev[video_id] = kzalloc(sizeof(*vdev), GFP_KERNEL);
  1884. if (WARN_ON(!vvdev[video_id])) {
  1885. rc = -ENOMEM;
  1886. goto probe_end;
  1887. }
  1888. vdev = vvdev[video_id];
  1889. vdev->id = video_id;
  1890. vdev->rmem = viv_find_isp_reserve_mem(vdev->id);
  1891. vdev->v4l2_dev = kzalloc(sizeof(*vdev->v4l2_dev), GFP_KERNEL);
  1892. if (WARN_ON(!vdev->v4l2_dev)) {
  1893. rc = -ENOMEM;
  1894. goto probe_end;
  1895. }
  1896. init_completion(&vdev->subscribed_wait);
  1897. vdev->subscribed_cnt = 0;
  1898. vdev->video = video_device_alloc();
  1899. vdev->video->v4l2_dev = vdev->v4l2_dev;
  1900. rc = v4l2_device_register(&pdev->dev, vdev->video->v4l2_dev);
  1901. if (WARN_ON(rc < 0))
  1902. goto register_fail;
  1903. sprintf(vdev->video->name, "viv_v4l2%d", video_id);
  1904. v4l2_ctrl_handler_init(&vdev->ctrls.handler, 1);
  1905. vdev->ctrls.request = v4l2_ctrl_new_custom(&vdev->ctrls.handler,
  1906. &viv_ext_ctrl, NULL);
  1907. create_controls(&vdev->ctrls.handler);
  1908. vdev->video->release = video_device_release;
  1909. vdev->video->fops = &video_ops;
  1910. vdev->video->ioctl_ops = &video_ioctl_ops;
  1911. vdev->video->minor = -1;
  1912. #if LINUX_VERSION_CODE > KERNEL_VERSION(5, 10, 0)
  1913. vdev->video->vfl_type = VFL_TYPE_VIDEO;
  1914. #else
  1915. vdev->video->vfl_type = VFL_TYPE_GRABBER;
  1916. #endif
  1917. vdev->video->ctrl_handler = &vdev->ctrls.handler;
  1918. #if LINUX_VERSION_CODE > KERNEL_VERSION(5, 0, 0)
  1919. vdev->video->device_caps =
  1920. V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING;
  1921. #endif
  1922. #ifdef ENABLE_IRQ
  1923. video_set_drvdata(vdev->video, vdev);
  1924. vvbuf_ctx_init(&vdev->bctx);
  1925. vdev->bctx.ops = &viv_buf_ops;
  1926. vdev->mdev = &mdev;
  1927. vdev->v4l2_dev->mdev = &mdev;
  1928. vdev->video->entity.name = vdev->video->name;
  1929. vdev->video->entity.obj_type = MEDIA_ENTITY_TYPE_VIDEO_DEVICE;
  1930. vdev->video->entity.function = MEDIA_ENT_F_IO_V4L;
  1931. vdev->video->entity.ops = &viv_media_ops;
  1932. vdev->pad.flags = MEDIA_PAD_FL_SINK | MEDIA_PAD_FL_MUST_CONNECT;
  1933. rc = media_entity_pads_init(&vdev->video->entity,
  1934. 1, &vdev->pad);
  1935. if (WARN_ON(rc < 0))
  1936. goto register_fail;
  1937. v4l2_async_notifier_init(&vdev->subdev_notifier);
  1938. vdev->subdev_notifier.ops = &sd_async_notifier_ops;
  1939. #endif
  1940. #if LINUX_VERSION_CODE > KERNEL_VERSION(5, 10, 0)
  1941. rc = video_register_device(vdev->video, VFL_TYPE_VIDEO, -1);
  1942. #else
  1943. rc = video_register_device(vdev->video, VFL_TYPE_GRABBER, -1);
  1944. #endif
  1945. if (WARN_ON(rc < 0))
  1946. goto register_fail;
  1947. #ifdef ENABLE_IRQ
  1948. for (j = 0; j < nodecount; ++j) {
  1949. if (nodes[j].id == video_id) {
  1950. switch (nodes[j].match_type) {
  1951. case V4L2_ASYNC_MATCH_FWNODE:
  1952. asd = v4l2_async_notifier_add_fwnode_subdev(
  1953. &vdev->subdev_notifier,
  1954. of_fwnode_handle(nodes[j].node),
  1955. sizeof(struct v4l2_async_subdev));
  1956. break;
  1957. case V4L2_ASYNC_MATCH_DEVNAME:
  1958. asd = v4l2_async_notifier_add_devname_subdev(
  1959. &vdev->subdev_notifier,
  1960. nodes[j].name,
  1961. sizeof(struct v4l2_async_subdev));
  1962. break;
  1963. default:
  1964. asd = NULL;
  1965. break;
  1966. }
  1967. if (asd) {
  1968. vdev->asd[vdev->asdcount] = asd;
  1969. vdev->asdcount++;
  1970. }
  1971. }
  1972. }
  1973. rc = v4l2_async_notifier_register(vdev->v4l2_dev,
  1974. &vdev->subdev_notifier);
  1975. if (WARN_ON(rc < 0))
  1976. goto register_fail;
  1977. #else
  1978. video_set_drvdata(vdev->video, vdev);
  1979. rc = v4l2_device_register_subdev_nodes(vdev->v4l2_dev);
  1980. #endif
  1981. vdev->ctrls.buf_va = kmalloc(VIV_JSON_BUFFER_SIZE, GFP_KERNEL);
  1982. vdev->ctrls.buf_pa = __pa(vdev->ctrls.buf_va);
  1983. init_completion(&vdev->ctrls.wait);
  1984. vdev->fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  1985. vdev->fmt.fmt.pix.field = V4L2_FIELD_NONE;
  1986. vdev->fmt.fmt.pix.colorspace = V4L2_COLORSPACE_REC709;
  1987. if (sizeof(vdev->formats) >= sizeof(formats)) {
  1988. memcpy(vdev->formats, formats, sizeof(formats));
  1989. vdev->formatscount = ARRAY_SIZE(formats);
  1990. }
  1991. for (m = 0; m < VIDEO_NODE_NUM; m++) {
  1992. vdev->loop_cnt[m] = 0;
  1993. vdev->frameCnt[m] = 0;
  1994. }
  1995. vdev->duration = 0;
  1996. vdev->last_ts = 0;
  1997. continue;
  1998. register_fail:
  1999. video_device_release(vdev->video);
  2000. }
  2001. }
  2002. #ifdef ENABLE_IRQ
  2003. if (!rc)
  2004. rc = media_device_register(&mdev);
  2005. #endif
  2006. probe_end:
  2007. return rc;
  2008. }
  2009. static int viv_video_remove(struct platform_device *pdev)
  2010. {
  2011. struct viv_video_device *vdev;
  2012. int i;
  2013. for (i = VIDEO_NODE_NUM-1; i >= 0; i--) {
  2014. vdev = vvdev[i];
  2015. if (!vdev || !vdev->video)
  2016. continue;
  2017. #ifdef ENABLE_IRQ
  2018. media_entity_cleanup(&vdev->video->entity);
  2019. v4l2_async_notifier_cleanup(&vdev->subdev_notifier);
  2020. v4l2_async_notifier_unregister(&vdev->subdev_notifier);
  2021. v4l2_device_unregister(vdev->v4l2_dev);
  2022. kfree(vdev->v4l2_dev);
  2023. #endif
  2024. video_unregister_device(vdev->video);
  2025. #ifdef ENABLE_IRQ
  2026. vvbuf_ctx_deinit(&vdev->bctx);
  2027. #else
  2028. v4l2_device_disconnect(vdev->video->v4l2_dev);
  2029. v4l2_device_put(vdev->video->v4l2_dev);
  2030. #endif
  2031. kfree(vdev->ctrls.buf_va);
  2032. v4l2_ctrl_handler_free(&vdev->ctrls.handler);
  2033. kfree(vvdev[i]);
  2034. vvdev[i] = NULL;
  2035. }
  2036. #ifdef ENABLE_IRQ
  2037. media_device_unregister(&mdev);
  2038. media_device_cleanup(&mdev);
  2039. #endif
  2040. return 0;
  2041. }
  2042. static struct platform_driver viv_video_driver = {
  2043. .probe = viv_video_probe,
  2044. .remove = viv_video_remove,
  2045. .driver = {
  2046. .name = "vvcam-video",
  2047. .owner = THIS_MODULE,
  2048. },
  2049. };
  2050. static int __init viv_video_init_module(void)
  2051. {
  2052. int ret = 0;
  2053. pr_info("enter %s\n", __func__);
  2054. ret = platform_device_register(&viv_pdev);
  2055. if (ret) {
  2056. pr_err("register platform device failed.\n");
  2057. return ret;
  2058. }
  2059. ret = platform_driver_register(&viv_video_driver);
  2060. if (ret) {
  2061. pr_err("register platform driver failed.\n");
  2062. platform_device_unregister(&viv_pdev);
  2063. return ret;
  2064. }
  2065. return ret;
  2066. }
  2067. static void __exit viv_video_exit_module(void)
  2068. {
  2069. pr_info("enter %s\n", __func__);
  2070. platform_driver_unregister(&viv_video_driver);
  2071. msleep(100);
  2072. platform_device_unregister(&viv_pdev);
  2073. }
  2074. module_init(viv_video_init_module);
  2075. module_exit(viv_video_exit_module);
  2076. MODULE_DESCRIPTION("Verisilicon V4L2 video driver");
  2077. MODULE_AUTHOR("Verisilicon ISP SW Team");
  2078. MODULE_LICENSE("GPL v2");