FreeBSD/Linux Kernel Cross Reference
sys/dev/video.c
1 /* $NetBSD: video.c,v 1.17.8.3 2009/03/15 20:34:29 snj Exp $ */
2
3 /*
4 * Copyright (c) 2008 Patrick Mahoney <pat@polycrystal.org>
5 * All rights reserved.
6 *
7 * This code was written by Patrick Mahoney (pat@polycrystal.org) as
8 * part of Google Summer of Code 2008.
9 *
10 * Redistribution and use in source and binary forms, with or without
11 * modification, are permitted provided that the following conditions
12 * are met:
13 * 1. Redistributions of source code must retain the above copyright
14 * notice, this list of conditions and the following disclaimer.
15 * 2. Redistributions in binary form must reproduce the above copyright
16 * notice, this list of conditions and the following disclaimer in the
17 * documentation and/or other materials provided with the distribution.
18 *
19 * THIS SOFTWARE IS PROVIDED BY THE NETBSD FOUNDATION, INC. AND CONTRIBUTORS
20 * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
21 * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
22 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE FOUNDATION OR CONTRIBUTORS
23 * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
24 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
25 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
26 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
27 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
28 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
29 * POSSIBILITY OF SUCH DAMAGE.
30 */
31
32 /*
33 * This ia a Video4Linux 2 compatible /dev/video driver for NetBSD
34 *
35 * See http://v4l2spec.bytesex.org/ for Video4Linux 2 specifications
36 */
37
38 #include <sys/cdefs.h>
39 __KERNEL_RCSID(0, "$NetBSD: video.c,v 1.17.8.3 2009/03/15 20:34:29 snj Exp $");
40
41 #include "video.h"
42 #if NVIDEO > 0
43
44 #include <sys/param.h>
45 #include <sys/ioctl.h>
46 #include <sys/fcntl.h>
47 #include <sys/vnode.h>
48 #include <sys/poll.h>
49 #include <sys/select.h>
50 #include <sys/kmem.h>
51 #include <sys/pool.h>
52 #include <sys/conf.h>
53 #include <sys/types.h>
54 #include <sys/device.h>
55 #include <sys/condvar.h>
56 #include <sys/queue.h>
57 #include <sys/videoio.h>
58
59 #include <dev/video_if.h>
60
61 /* #define VIDEO_DEBUG 1 */
62
63 #ifdef VIDEO_DEBUG
64 #define DPRINTF(x) do { if (videodebug) printf x; } while (0)
65 #define DPRINTFN(n,x) do { if (videodebug>(n)) printf x; } while (0)
66 int videodebug = VIDEO_DEBUG;
67 #else
68 #define DPRINTF(x)
69 #define DPRINTFN(n,x)
70 #endif
71
72 #define VIDEO_DRIVER_VERSION 1
73
74 /* TODO: move to sys/intr.h */
75 #define IPL_VIDEO IPL_VM
76 #define splvideo() splvm()
77
78 #define VIDEO_MIN_BUFS 2
79 #define VIDEO_MAX_BUFS 32
80 #define VIDEO_NUM_BUFS 4
81
82 /* Scatter Buffer - an array of fixed size (PAGE_SIZE) chunks
83 * allocated non-contiguously and functions to get data into and out
84 * of the scatter buffer. */
85 struct scatter_buf {
86 pool_cache_t sb_pool;
87 size_t sb_size; /* size in bytes */
88 size_t sb_npages; /* number of pages */
89 uint8_t **sb_page_ary; /* array of page pointers */
90 };
91
92 struct scatter_io {
93 struct scatter_buf *sio_buf;
94 off_t sio_offset;
95 size_t sio_resid;
96 };
97
98 static void scatter_buf_init(struct scatter_buf *);
99 static void scatter_buf_destroy(struct scatter_buf *);
100 static int scatter_buf_set_size(struct scatter_buf *, size_t);
101 static paddr_t scatter_buf_map(struct scatter_buf *, off_t);
102
103 static bool scatter_io_init(struct scatter_buf *, off_t, size_t, struct scatter_io *);
104 static bool scatter_io_next(struct scatter_io *, void **, size_t *);
105 static void scatter_io_undo(struct scatter_io *, size_t);
106 static void scatter_io_copyin(struct scatter_io *, const void *);
107 /* static void scatter_io_copyout(struct scatter_io *, void *); */
108 static int scatter_io_uiomove(struct scatter_io *, struct uio *);
109
110
111 enum video_stream_method {
112 VIDEO_STREAM_METHOD_NONE,
113 VIDEO_STREAM_METHOD_READ,
114 VIDEO_STREAM_METHOD_MMAP,
115 VIDEO_STREAM_METHOD_USERPTR
116 };
117
118 struct video_buffer {
119 struct v4l2_buffer *vb_buf;
120 SIMPLEQ_ENTRY(video_buffer) entries;
121 };
122
123 SIMPLEQ_HEAD(sample_queue, video_buffer);
124
125 struct video_stream {
126 int vs_flags; /* flags given to open() */
127
128 struct video_format vs_format;
129
130 int vs_frameno; /* toggles between 0 and 1,
131 * or -1 if new */
132 uint32_t vs_sequence; /* absoulte frame/sample number in
133 * sequence, wraps around */
134 bool vs_drop; /* drop payloads from current
135 * frameno? */
136
137 enum v4l2_buf_type vs_type;
138 uint8_t vs_nbufs;
139 struct video_buffer **vs_buf;
140
141 struct scatter_buf vs_data; /* stores video data for MMAP
142 * and READ */
143
144 /* Video samples may exist in different locations. Initially,
145 * samples are queued into the ingress queue. The driver
146 * grabs these in turn and fills them with video data. Once
147 * filled, they are moved to the egress queue. Samples are
148 * dequeued either by user with MMAP method or, with READ
149 * method, videoread() works from the fist sample in the
150 * ingress queue without dequeing. In the first case, the
151 * user re-queues the buffer when finished, and videoread()
152 * does the same when all data has been read. The sample now
153 * returns to the ingress queue. */
154 struct sample_queue vs_ingress; /* samples under driver control */
155 struct sample_queue vs_egress; /* samples headed for userspace */
156
157 bool vs_streaming;
158 enum video_stream_method vs_method; /* method by which
159 * userspace will read
160 * samples */
161
162 kmutex_t vs_lock; /* Lock to manipulate queues.
163 * Should also be held when
164 * changing number of
165 * buffers. */
166 kcondvar_t vs_sample_cv; /* signaled on new
167 * ingress sample */
168 struct selinfo vs_sel;
169
170 uint32_t vs_bytesread; /* bytes read() from current
171 * sample thus far */
172 };
173
174 struct video_softc {
175 device_t sc_dev;
176 device_t hw_dev; /* Hardware (parent) device */
177 void * hw_softc; /* Hardware device private softc */
178 const struct video_hw_if *hw_if; /* Hardware interface */
179
180 u_int sc_open;
181 int sc_refcnt;
182 int sc_opencnt;
183 bool sc_dying;
184
185 struct video_stream sc_stream_in;
186 };
187 static int video_print(void *, const char *);
188
189 static int video_match(device_t, cfdata_t, void *);
190 static void video_attach(device_t, device_t, void *);
191 static int video_detach(device_t, int);
192 static int video_activate(device_t, enum devact);
193
194 dev_type_open(videoopen);
195 dev_type_close(videoclose);
196 dev_type_read(videoread);
197 dev_type_write(videowrite);
198 dev_type_ioctl(videoioctl);
199 dev_type_poll(videopoll);
200 dev_type_mmap(videommap);
201
202 const struct cdevsw video_cdevsw = {
203 videoopen, videoclose, videoread, videowrite, videoioctl,
204 nostop, notty, videopoll, videommap, nokqfilter, D_OTHER
205 };
206
207 #define VIDEOUNIT(n) (minor(n))
208
209 CFATTACH_DECL_NEW(video, sizeof(struct video_softc),
210 video_match, video_attach, video_detach, video_activate);
211
212 extern struct cfdriver video_cd;
213
214 static const char * video_pixel_format_str(enum video_pixel_format);
215
216 /* convert various values from V4L2 to native values of this driver */
217 static uint16_t v4l2id_to_control_id(uint32_t);
218 static uint32_t control_flags_to_v4l2flags(uint32_t);
219 static enum v4l2_ctrl_type control_type_to_v4l2type(enum video_control_type);
220
221 static void v4l2_format_to_video_format(const struct v4l2_format *,
222 struct video_format *);
223 static void video_format_to_v4l2_format(const struct video_format *,
224 struct v4l2_format *);
225
226 /* V4L2 api functions, typically called from videoioclt() */
227 static int video_enum_format(struct video_softc *, struct v4l2_fmtdesc *);
228 static int video_get_format(struct video_softc *,
229 struct v4l2_format *);
230 static int video_set_format(struct video_softc *,
231 struct v4l2_format *);
232 static int video_try_format(struct video_softc *,
233 struct v4l2_format *);
234 static int video_query_control(struct video_softc *,
235 struct v4l2_queryctrl *);
236 static int video_get_control(struct video_softc *,
237 struct v4l2_control *);
238 static int video_set_control(struct video_softc *,
239 const struct v4l2_control *);
240 static int video_request_bufs(struct video_softc *,
241 struct v4l2_requestbuffers *);
242 static int video_query_buf(struct video_softc *, struct v4l2_buffer *);
243 static int video_queue_buf(struct video_softc *, struct v4l2_buffer *);
244 static int video_dequeue_buf(struct video_softc *, struct v4l2_buffer *);
245 static int video_stream_on(struct video_softc *, enum v4l2_buf_type);
246 static int video_stream_off(struct video_softc *, enum v4l2_buf_type);
247
248 static struct video_buffer * video_buffer_alloc(void);
249 static void video_buffer_free(struct video_buffer *);
250
251
252 /* functions for video_stream */
253 static void video_stream_init(struct video_stream *);
254 static void video_stream_fini(struct video_stream *);
255
256 static int video_stream_setup_bufs(struct video_stream *,
257 enum video_stream_method,
258 uint8_t);
259 static void video_stream_teardown_bufs(struct video_stream *);
260
261 static int video_stream_realloc_bufs(struct video_stream *, uint8_t);
262 #define video_stream_free_bufs(vs) \
263 video_stream_realloc_bufs((vs), 0)
264
265 static void video_stream_enqueue(struct video_stream *,
266 struct video_buffer *);
267 static struct video_buffer * video_stream_dequeue(struct video_stream *);
268 static void video_stream_write(struct video_stream *,
269 const struct video_payload *);
270 static void video_stream_sample_done(struct video_stream *);
271
272 #ifdef VIDEO_DEBUG
273 /* debugging */
274 static const char * video_ioctl_str(u_long);
275 #endif
276
277
278 static int
279 video_match(device_t parent, cfdata_t match, void *aux)
280 {
281 struct video_attach_args *args;
282
283 args = aux;
284 DPRINTF(("video_match: hw=%p\n", args->hw_if));
285 return 1;
286 }
287
288
289 static void
290 video_attach(device_t parent, device_t self, void *aux)
291 {
292 struct video_softc *sc;
293 struct video_attach_args *args;
294
295 sc = device_private(self);
296 args = aux;
297
298 sc->sc_dev = self;
299 sc->hw_dev = parent;
300 sc->hw_if = args->hw_if;
301 sc->hw_softc = device_private(parent);
302
303 sc->sc_open = 0;
304 sc->sc_refcnt = 0;
305 sc->sc_opencnt = 0;
306 sc->sc_dying = false;
307
308 video_stream_init(&sc->sc_stream_in);
309
310 aprint_naive("\n");
311 aprint_normal(": %s\n", sc->hw_if->get_devname(sc->hw_softc));
312
313 DPRINTF(("video_attach: sc=%p hwif=%p\n", sc, sc->hw_if));
314
315 if (!pmf_device_register(self, NULL, NULL))
316 aprint_error_dev(self, "couldn't establish power handler\n");
317 }
318
319
320 static int
321 video_activate(device_t self, enum devact act)
322 {
323 struct video_softc *sc;
324
325 sc = device_private(self);
326 DPRINTF(("video_activate: sc=%p\n", sc));
327 switch (act) {
328 case DVACT_ACTIVATE:
329 return EOPNOTSUPP;
330
331 case DVACT_DEACTIVATE:
332 sc->sc_dying = true;
333 break;
334 }
335 return 0;
336 }
337
338
339 static int
340 video_detach(device_t self, int flags)
341 {
342 struct video_softc *sc;
343 int maj, mn;
344
345 sc = device_private(self);
346 DPRINTF(("video_detach: sc=%p flags=%d\n", sc, flags));
347
348 sc->sc_dying = true;
349
350 pmf_device_deregister(self);
351
352 maj = cdevsw_lookup_major(&video_cdevsw);
353 mn = device_unit(self);
354 /* close open instances */
355 vdevgone(maj, mn, mn, VCHR);
356
357 video_stream_fini(&sc->sc_stream_in);
358
359 return 0;
360 }
361
362
363 static int
364 video_print(void *aux, const char *pnp)
365 {
366 struct video_attach_args *arg;
367
368 if (pnp != NULL) {
369 DPRINTF(("video_print: have pnp\n"));
370 arg = aux;
371 aprint_normal("%s at %s\n", "video", pnp);
372 } else {
373 DPRINTF(("video_print: pnp is NULL\n"));
374 }
375 return UNCONF;
376 }
377
378
379 /*
380 * Called from hardware driver. This is where the MI audio driver
381 * gets probed/attached to the hardware driver.
382 */
383 device_t
384 video_attach_mi(const struct video_hw_if *hw_if, device_t parent)
385 {
386 struct video_attach_args args;
387
388 args.hw_if = hw_if;
389 return config_found_ia(parent, "videobus", &args, video_print);
390 }
391
392 /* video_submit_payload - called by hardware driver to submit payload data */
393 void
394 video_submit_payload(device_t self, const struct video_payload *payload)
395 {
396 struct video_softc *sc;
397
398 sc = device_private(self);
399
400 if (sc == NULL)
401 return;
402
403 video_stream_write(&sc->sc_stream_in, payload);
404 }
405
406 static const char *
407 video_pixel_format_str(enum video_pixel_format px)
408 {
409 switch (px) {
410 case VIDEO_FORMAT_UYVY: return "UYVY";
411 case VIDEO_FORMAT_YUV420: return "YUV420";
412 case VIDEO_FORMAT_YUY2: return "YUYV";
413 case VIDEO_FORMAT_NV12: return "NV12";
414 case VIDEO_FORMAT_RGB24: return "RGB24";
415 case VIDEO_FORMAT_RGB555: return "RGB555";
416 case VIDEO_FORMAT_RGB565: return "RGB565";
417 case VIDEO_FORMAT_SBGGR8: return "SBGGR8";
418 case VIDEO_FORMAT_MJPEG: return "MJPEG";
419 case VIDEO_FORMAT_DV: return "DV";
420 case VIDEO_FORMAT_MPEG: return "MPEG";
421 default: return "Unknown";
422 }
423 }
424
425 /* Takes a V4L2 id and returns a "native" video driver control id.
426 * TODO: is there a better way to do this? some kind of array? */
427 static uint16_t
428 v4l2id_to_control_id(uint32_t v4l2id)
429 {
430 /* mask includes class bits and control id bits */
431 switch (v4l2id & 0xffffff) {
432 case V4L2_CID_BRIGHTNESS: return VIDEO_CONTROL_BRIGHTNESS;
433 case V4L2_CID_CONTRAST: return VIDEO_CONTROL_CONTRAST;
434 case V4L2_CID_SATURATION: return VIDEO_CONTROL_SATURATION;
435 case V4L2_CID_HUE: return VIDEO_CONTROL_HUE;
436 case V4L2_CID_HUE_AUTO: return VIDEO_CONTROL_HUE_AUTO;
437 case V4L2_CID_SHARPNESS: return VIDEO_CONTROL_SHARPNESS;
438 case V4L2_CID_GAMMA: return VIDEO_CONTROL_GAMMA;
439
440 /* "black level" means the same as "brightness", but V4L2
441 * defines two separate controls that are not identical.
442 * V4L2_CID_BLACK_LEVEL is deprecated however in V4L2. */
443 case V4L2_CID_BLACK_LEVEL: return VIDEO_CONTROL_BRIGHTNESS;
444
445 case V4L2_CID_AUDIO_VOLUME: return VIDEO_CONTROL_UNDEFINED;
446 case V4L2_CID_AUDIO_BALANCE: return VIDEO_CONTROL_UNDEFINED;
447 case V4L2_CID_AUDIO_BASS: return VIDEO_CONTROL_UNDEFINED;
448 case V4L2_CID_AUDIO_TREBLE: return VIDEO_CONTROL_UNDEFINED;
449 case V4L2_CID_AUDIO_MUTE: return VIDEO_CONTROL_UNDEFINED;
450 case V4L2_CID_AUDIO_LOUDNESS: return VIDEO_CONTROL_UNDEFINED;
451
452 case V4L2_CID_AUTO_WHITE_BALANCE:
453 return VIDEO_CONTROL_WHITE_BALANCE_AUTO;
454 case V4L2_CID_DO_WHITE_BALANCE:
455 return VIDEO_CONTROL_WHITE_BALANCE_ACTION;
456 case V4L2_CID_RED_BALANCE:
457 case V4L2_CID_BLUE_BALANCE:
458 /* This might not fit in with the control_id/value_id scheme */
459 return VIDEO_CONTROL_WHITE_BALANCE_COMPONENT;
460 case V4L2_CID_WHITE_BALANCE_TEMPERATURE:
461 return VIDEO_CONTROL_WHITE_BALANCE_TEMPERATURE;
462 case V4L2_CID_EXPOSURE:
463 return VIDEO_CONTROL_EXPOSURE_TIME_ABSOLUTE;
464 case V4L2_CID_GAIN: return VIDEO_CONTROL_GAIN;
465 case V4L2_CID_AUTOGAIN: return VIDEO_CONTROL_GAIN_AUTO;
466 case V4L2_CID_HFLIP: return VIDEO_CONTROL_HFLIP;
467 case V4L2_CID_VFLIP: return VIDEO_CONTROL_VFLIP;
468 case V4L2_CID_HCENTER_DEPRECATED:
469 case V4L2_CID_VCENTER_DEPRECATED:
470 return VIDEO_CONTROL_UNDEFINED;
471 case V4L2_CID_POWER_LINE_FREQUENCY:
472 return VIDEO_CONTROL_POWER_LINE_FREQUENCY;
473 case V4L2_CID_BACKLIGHT_COMPENSATION:
474 return VIDEO_CONTROL_BACKLIGHT_COMPENSATION;
475 default: return V4L2_CTRL_ID2CID(v4l2id);
476 }
477 }
478
479
480 static uint32_t
481 control_flags_to_v4l2flags(uint32_t flags)
482 {
483 uint32_t v4l2flags = 0;
484
485 if (flags & VIDEO_CONTROL_FLAG_DISABLED)
486 v4l2flags |= V4L2_CTRL_FLAG_INACTIVE;
487
488 if (!(flags & VIDEO_CONTROL_FLAG_WRITE))
489 v4l2flags |= V4L2_CTRL_FLAG_READ_ONLY;
490
491 if (flags & VIDEO_CONTROL_FLAG_AUTOUPDATE)
492 v4l2flags |= V4L2_CTRL_FLAG_GRABBED;
493
494 return v4l2flags;
495 }
496
497
498 static enum v4l2_ctrl_type
499 control_type_to_v4l2type(enum video_control_type type) {
500 switch (type) {
501 case VIDEO_CONTROL_TYPE_INT: return V4L2_CTRL_TYPE_INTEGER;
502 case VIDEO_CONTROL_TYPE_BOOL: return V4L2_CTRL_TYPE_BOOLEAN;
503 case VIDEO_CONTROL_TYPE_LIST: return V4L2_CTRL_TYPE_MENU;
504 case VIDEO_CONTROL_TYPE_ACTION: return V4L2_CTRL_TYPE_BUTTON;
505 default: return V4L2_CTRL_TYPE_INTEGER; /* err? */
506 }
507 }
508
509
510 static int
511 video_query_control(struct video_softc *sc,
512 struct v4l2_queryctrl *query)
513 {
514 const struct video_hw_if *hw;
515 struct video_control_desc_group desc_group;
516 struct video_control_desc desc;
517 int err;
518
519 hw = sc->hw_if;
520 if (hw->get_control_desc_group) {
521 desc.group_id = desc.control_id =
522 v4l2id_to_control_id(query->id);
523
524 desc_group.group_id = desc.group_id;
525 desc_group.length = 1;
526 desc_group.desc = &desc;
527
528 err = hw->get_control_desc_group(sc->hw_softc, &desc_group);
529 if (err != 0)
530 return err;
531
532 query->type = control_type_to_v4l2type(desc.type);
533 memcpy(query->name, desc.name, 32);
534 query->minimum = desc.min;
535 query->maximum = desc.max;
536 query->step = desc.step;
537 query->default_value = desc.def;
538 query->flags = control_flags_to_v4l2flags(desc.flags);
539
540 return 0;
541 } else {
542 return EINVAL;
543 }
544 }
545
546
547 /* Takes a single Video4Linux2 control and queries the driver for the
548 * current value. */
549 static int
550 video_get_control(struct video_softc *sc,
551 struct v4l2_control *vcontrol)
552 {
553 const struct video_hw_if *hw;
554 struct video_control_group group;
555 struct video_control control;
556 int err;
557
558 hw = sc->hw_if;
559 if (hw->get_control_group) {
560 control.group_id = control.control_id =
561 v4l2id_to_control_id(vcontrol->id);
562 /* ?? if "control_id" is arbitrarily defined by the
563 * driver, then we need some way to store it... Maybe
564 * it doesn't matter for single value controls. */
565 control.value = 0;
566
567 group.group_id = control.group_id;
568 group.length = 1;
569 group.control = &control;
570
571 err = hw->get_control_group(sc->hw_softc, &group);
572 if (err != 0)
573 return err;
574
575 vcontrol->value = control.value;
576 return 0;
577 } else {
578 return EINVAL;
579 }
580 }
581
582 static void
583 video_format_to_v4l2_format(const struct video_format *src,
584 struct v4l2_format *dest)
585 {
586 /* TODO: what about win and vbi formats? */
587 dest->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
588 dest->fmt.pix.width = src->width;
589 dest->fmt.pix.height = src->height;
590 dest->fmt.pix.field = V4L2_FIELD_NONE; /* TODO: for now,
591 * just set to
592 * progressive */
593 dest->fmt.pix.bytesperline = src->stride;
594 dest->fmt.pix.sizeimage = src->sample_size;
595 dest->fmt.pix.colorspace = 0; /* XXX */
596 dest->fmt.pix.priv = src->priv;
597
598 switch (src->pixel_format) {
599 case VIDEO_FORMAT_UYVY:
600 dest->fmt.pix.pixelformat = V4L2_PIX_FMT_UYVY;
601 break;
602 case VIDEO_FORMAT_YUV420:
603 dest->fmt.pix.pixelformat = V4L2_PIX_FMT_YUV420;
604 break;
605 case VIDEO_FORMAT_YUY2:
606 dest->fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
607 break;
608 case VIDEO_FORMAT_NV12:
609 dest->fmt.pix.pixelformat = V4L2_PIX_FMT_NV12;
610 break;
611 case VIDEO_FORMAT_RGB24:
612 dest->fmt.pix.pixelformat = V4L2_PIX_FMT_RGB24;
613 break;
614 case VIDEO_FORMAT_RGB555:
615 dest->fmt.pix.pixelformat = V4L2_PIX_FMT_RGB555;
616 break;
617 case VIDEO_FORMAT_RGB565:
618 dest->fmt.pix.pixelformat = V4L2_PIX_FMT_RGB565;
619 break;
620 case VIDEO_FORMAT_SBGGR8:
621 dest->fmt.pix.pixelformat = V4L2_PIX_FMT_SBGGR8;
622 break;
623 case VIDEO_FORMAT_MJPEG:
624 dest->fmt.pix.pixelformat = V4L2_PIX_FMT_MJPEG;
625 break;
626 case VIDEO_FORMAT_DV:
627 dest->fmt.pix.pixelformat = V4L2_PIX_FMT_DV;
628 break;
629 case VIDEO_FORMAT_MPEG:
630 dest->fmt.pix.pixelformat = V4L2_PIX_FMT_MPEG;
631 break;
632 case VIDEO_FORMAT_UNDEFINED:
633 default:
634 DPRINTF(("video_get_format: unknown pixel format %d\n",
635 src->pixel_format));
636 dest->fmt.pix.pixelformat = 0; /* V4L2 doesn't define
637 * and "undefined"
638 * format? */
639 break;
640 }
641
642 }
643
644 static void
645 v4l2_format_to_video_format(const struct v4l2_format *src,
646 struct video_format *dest)
647 {
648 switch (src->type) {
649 case V4L2_BUF_TYPE_VIDEO_CAPTURE:
650 dest->width = src->fmt.pix.width;
651 dest->height = src->fmt.pix.height;
652
653 dest->stride = src->fmt.pix.bytesperline;
654 dest->sample_size = src->fmt.pix.sizeimage;
655
656 switch (src->fmt.pix.pixelformat) {
657 case V4L2_PIX_FMT_UYVY:
658 dest->pixel_format = VIDEO_FORMAT_UYVY;
659 break;
660 case V4L2_PIX_FMT_YUV420:
661 dest->pixel_format = VIDEO_FORMAT_YUV420;
662 break;
663 case V4L2_PIX_FMT_YUYV:
664 dest->pixel_format = VIDEO_FORMAT_YUY2;
665 break;
666 case V4L2_PIX_FMT_NV12:
667 dest->pixel_format = VIDEO_FORMAT_NV12;
668 break;
669 case V4L2_PIX_FMT_RGB24:
670 dest->pixel_format = VIDEO_FORMAT_RGB24;
671 break;
672 case V4L2_PIX_FMT_RGB555:
673 dest->pixel_format = VIDEO_FORMAT_RGB555;
674 break;
675 case V4L2_PIX_FMT_RGB565:
676 dest->pixel_format = VIDEO_FORMAT_RGB565;
677 break;
678 case V4L2_PIX_FMT_SBGGR8:
679 dest->pixel_format = VIDEO_FORMAT_SBGGR8;
680 break;
681 case V4L2_PIX_FMT_MJPEG:
682 dest->pixel_format = VIDEO_FORMAT_MJPEG;
683 break;
684 case V4L2_PIX_FMT_DV:
685 dest->pixel_format = VIDEO_FORMAT_DV;
686 break;
687 case V4L2_PIX_FMT_MPEG:
688 dest->pixel_format = VIDEO_FORMAT_MPEG;
689 break;
690 default:
691 DPRINTF(("video: unknown v4l2 pixel format %d\n",
692 src->fmt.pix.pixelformat));
693 dest->pixel_format = VIDEO_FORMAT_UNDEFINED;
694 break;
695 }
696 break;
697 default:
698 /* TODO: other v4l2 format types */
699 DPRINTF(("video: unsupported v4l2 format type %d\n",
700 src->type));
701 break;
702 }
703 }
704
705 static int
706 video_enum_format(struct video_softc *sc, struct v4l2_fmtdesc *fmtdesc)
707 {
708 const struct video_hw_if *hw;
709 struct video_format vfmt;
710 struct v4l2_format fmt;
711 int err;
712
713 hw = sc->hw_if;
714 if (hw->enum_format == NULL)
715 return ENOTTY;
716
717 err = hw->enum_format(sc->hw_softc, fmtdesc->index, &vfmt);
718 if (err != 0)
719 return err;
720
721 video_format_to_v4l2_format(&vfmt, &fmt);
722
723 fmtdesc->type = V4L2_BUF_TYPE_VIDEO_CAPTURE; /* TODO: only one type for now */
724 if (vfmt.pixel_format >= VIDEO_FORMAT_MJPEG)
725 fmtdesc->flags = V4L2_FMT_FLAG_COMPRESSED;
726 strlcpy(fmtdesc->description,
727 video_pixel_format_str(vfmt.pixel_format),
728 sizeof(fmtdesc->description));
729 fmtdesc->pixelformat = fmt.fmt.pix.pixelformat;
730
731 return 0;
732 }
733
734 static int
735 video_get_format(struct video_softc *sc,
736 struct v4l2_format *format)
737 {
738 const struct video_hw_if *hw;
739 struct video_format vfmt;
740 int err;
741
742 hw = sc->hw_if;
743 if (hw->get_format == NULL)
744 return ENOTTY;
745
746 err = hw->get_format(sc->hw_softc, &vfmt);
747 if (err != 0)
748 return err;
749
750 video_format_to_v4l2_format(&vfmt, format);
751
752 return 0;
753 }
754
755 static int
756 video_set_format(struct video_softc *sc, struct v4l2_format *fmt)
757 {
758 const struct video_hw_if *hw;
759 struct video_format vfmt;
760 int err;
761
762 hw = sc->hw_if;
763 if (hw->set_format == NULL)
764 return ENOTTY;
765
766 v4l2_format_to_video_format(fmt, &vfmt);
767
768 err = hw->set_format(sc->hw_softc, &vfmt);
769 if (err != 0)
770 return err;
771
772 video_format_to_v4l2_format(&vfmt, fmt);
773 sc->sc_stream_in.vs_format = vfmt;
774
775 return 0;
776 }
777
778
779 static int
780 video_try_format(struct video_softc *sc,
781 struct v4l2_format *format)
782 {
783 const struct video_hw_if *hw;
784 struct video_format vfmt;
785 int err;
786
787 hw = sc->hw_if;
788 if (hw->try_format == NULL)
789 return ENOTTY;
790
791 v4l2_format_to_video_format(format, &vfmt);
792
793 err = hw->try_format(sc->hw_softc, &vfmt);
794 if (err != 0)
795 return err;
796
797 video_format_to_v4l2_format(&vfmt, format);
798
799 return 0;
800 }
801
802 /* Takes a single Video4Linux2 control, converts it to a struct
803 * video_control, and calls the hardware driver. */
804 static int
805 video_set_control(struct video_softc *sc,
806 const struct v4l2_control *vcontrol)
807 {
808 const struct video_hw_if *hw;
809 struct video_control_group group;
810 struct video_control control;
811
812 hw = sc->hw_if;
813 if (hw->set_control_group) {
814 control.group_id = control.control_id =
815 v4l2id_to_control_id(vcontrol->id);
816 /* ?? if "control_id" is arbitrarily defined by the
817 * driver, then we need some way to store it... Maybe
818 * it doesn't matter for single value controls. */
819 control.value = vcontrol->value;
820
821 group.group_id = control.group_id;
822 group.length = 1;
823 group.control = &control;
824
825 return (hw->set_control_group(sc->hw_softc, &group));
826 } else {
827 return EINVAL;
828 }
829 }
830
831 static int
832 video_request_bufs(struct video_softc *sc,
833 struct v4l2_requestbuffers *req)
834 {
835 struct video_stream *vs = &sc->sc_stream_in;
836 struct v4l2_buffer *buf;
837 int i, err;
838
839 if (req->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
840 return EINVAL;
841
842 vs->vs_type = req->type;
843
844 switch (req->memory) {
845 case V4L2_MEMORY_MMAP:
846 if (req->count < VIDEO_MIN_BUFS)
847 req->count = VIDEO_MIN_BUFS;
848 else if (req->count > VIDEO_MAX_BUFS)
849 req->count = VIDEO_MAX_BUFS;
850
851 err = video_stream_setup_bufs(vs,
852 VIDEO_STREAM_METHOD_MMAP,
853 req->count);
854 if (err != 0)
855 return err;
856
857 for (i = 0; i < req->count; ++i) {
858 buf = vs->vs_buf[i]->vb_buf;
859 buf->memory = V4L2_MEMORY_MMAP;
860 buf->flags |= V4L2_BUF_FLAG_MAPPED;
861 }
862 break;
863 case V4L2_MEMORY_USERPTR:
864 default:
865 return EINVAL;
866 }
867
868 return 0;
869 }
870
871 static int
872 video_query_buf(struct video_softc *sc,
873 struct v4l2_buffer *buf)
874 {
875 struct video_stream *vs = &sc->sc_stream_in;
876
877 if (buf->type != vs->vs_type)
878 return EINVAL;
879 if (buf->index >= vs->vs_nbufs)
880 return EINVAL;
881
882 memcpy(buf, vs->vs_buf[buf->index]->vb_buf, sizeof(*buf));
883
884 return 0;
885 }
886
887 /* Accept a buffer descriptor from userspace and return the indicated
888 * buffer to the driver's queue. */
889 static int
890 video_queue_buf(struct video_softc *sc, struct v4l2_buffer *userbuf)
891 {
892 struct video_stream *vs = &sc->sc_stream_in;
893 struct video_buffer *vb;
894 struct v4l2_buffer *driverbuf;
895
896 if (userbuf->type != vs->vs_type) {
897 DPRINTF(("video_queue_buf: expected type=%d got type=%d\n",
898 userbuf->type, vs->vs_type));
899 return EINVAL;
900 }
901 if (userbuf->index >= vs->vs_nbufs) {
902 DPRINTF(("video_queue_buf: invalid index %d >= %d\n",
903 userbuf->index, vs->vs_nbufs));
904 return EINVAL;
905 }
906
907 switch (vs->vs_method) {
908 case VIDEO_STREAM_METHOD_MMAP:
909 if (userbuf->memory != V4L2_MEMORY_MMAP) {
910 DPRINTF(("video_queue_buf: invalid memory=%d\n",
911 userbuf->memory));
912 return EINVAL;
913 }
914
915 mutex_enter(&vs->vs_lock);
916
917 vb = vs->vs_buf[userbuf->index];
918 driverbuf = vb->vb_buf;
919 if (driverbuf->flags & V4L2_BUF_FLAG_QUEUED) {
920 DPRINTF(("video_queue_buf: buf already queued; "
921 "flags=0x%x\n", driverbuf->flags));
922 mutex_exit(&vs->vs_lock);
923 return EINVAL;
924 }
925 video_stream_enqueue(vs, vb);
926 memcpy(userbuf, driverbuf, sizeof(*driverbuf));
927
928 mutex_exit(&vs->vs_lock);
929 break;
930 default:
931 return EINVAL;
932 }
933
934 return 0;
935 }
936
937 /* Dequeue the described buffer from the driver queue, making it
938 * available for reading via mmap. */
939 static int
940 video_dequeue_buf(struct video_softc *sc, struct v4l2_buffer *buf)
941 {
942 struct video_stream *vs = &sc->sc_stream_in;
943 struct video_buffer *vb;
944 int err;
945
946 if (buf->type != vs->vs_type) {
947 aprint_debug_dev(sc->sc_dev,
948 "requested type %d (expected %d)\n",
949 buf->type, vs->vs_type);
950 return EINVAL;
951 }
952
953 switch (vs->vs_method) {
954 case VIDEO_STREAM_METHOD_MMAP:
955 if (buf->memory != V4L2_MEMORY_MMAP) {
956 aprint_debug_dev(sc->sc_dev,
957 "requested memory %d (expected %d)\n",
958 buf->memory, V4L2_MEMORY_MMAP);
959 return EINVAL;
960 }
961
962 mutex_enter(&vs->vs_lock);
963
964 if (vs->vs_flags & O_NONBLOCK) {
965 vb = video_stream_dequeue(vs);
966 if (vb == NULL) {
967 mutex_exit(&vs->vs_lock);
968 return EAGAIN;
969 }
970 } else {
971 /* Block until we have sample */
972 while ((vb = video_stream_dequeue(vs)) == NULL) {
973 if (!vs->vs_streaming) {
974 mutex_exit(&vs->vs_lock);
975 return EINVAL;
976 }
977 err = cv_wait_sig(&vs->vs_sample_cv,
978 &vs->vs_lock);
979 if (err != 0) {
980 mutex_exit(&vs->vs_lock);
981 return EINTR;
982 }
983 }
984 }
985
986 memcpy(buf, vb->vb_buf, sizeof(*buf));
987
988 mutex_exit(&vs->vs_lock);
989 break;
990 default:
991 aprint_debug_dev(sc->sc_dev, "unknown vs_method %d\n",
992 vs->vs_method);
993 return EINVAL;
994 }
995
996 return 0;
997 }
998
999 static int
1000 video_stream_on(struct video_softc *sc, enum v4l2_buf_type type)
1001 {
1002 int err;
1003 struct video_stream *vs = &sc->sc_stream_in;
1004 const struct video_hw_if *hw;
1005
1006 if (vs->vs_streaming)
1007 return 0;
1008 if (type != vs->vs_type)
1009 return EINVAL;
1010
1011 hw = sc->hw_if;
1012 if (hw == NULL)
1013 return ENXIO;
1014
1015
1016 err = hw->start_transfer(sc->hw_softc);
1017 if (err != 0)
1018 return err;
1019
1020 vs->vs_streaming = true;
1021 return 0;
1022 }
1023
1024 static int
1025 video_stream_off(struct video_softc *sc, enum v4l2_buf_type type)
1026 {
1027 int err;
1028 struct video_stream *vs = &sc->sc_stream_in;
1029 const struct video_hw_if *hw;
1030
1031 if (!vs->vs_streaming)
1032 return 0;
1033 if (type != vs->vs_type)
1034 return EINVAL;
1035
1036 hw = sc->hw_if;
1037 if (hw == NULL)
1038 return ENXIO;
1039
1040 err = hw->stop_transfer(sc->hw_softc);
1041 if (err != 0)
1042 return err;
1043
1044 vs->vs_frameno = -1;
1045 vs->vs_sequence = 0;
1046 vs->vs_streaming = false;
1047
1048 return 0;
1049 }
1050
1051 int
1052 videoopen(dev_t dev, int flags, int ifmt, struct lwp *l)
1053 {
1054 struct video_softc *sc;
1055 const struct video_hw_if *hw;
1056 struct video_stream *vs;
1057 int err;
1058
1059 DPRINTF(("videoopen\n"));
1060
1061 sc = device_private(device_lookup(&video_cd, VIDEOUNIT(dev)));
1062 if (sc == NULL) {
1063 DPRINTF(("videoopen: failed to get softc\n"));
1064 return ENXIO;
1065 }
1066
1067 if (sc->sc_dying) {
1068 DPRINTF(("videoopen: dying\n"));
1069 return EIO;
1070 }
1071
1072 sc->sc_stream_in.vs_flags = flags;
1073
1074 DPRINTF(("videoopen: flags=0x%x sc=%p parent=%p\n",
1075 flags, sc, sc->hw_dev));
1076
1077 hw = sc->hw_if;
1078 if (hw == NULL)
1079 return ENXIO;
1080
1081 device_active(sc->sc_dev, DVA_SYSTEM);
1082
1083 sc->sc_opencnt++;
1084
1085 if (hw->open != NULL) {
1086 err = hw->open(sc->hw_softc, flags);
1087 if (err)
1088 return err;
1089 }
1090
1091 /* set up input stream. TODO: check flags to determine if
1092 * "read" is desired? */
1093 vs = &sc->sc_stream_in;
1094
1095 if (hw->get_format != NULL) {
1096 err = hw->get_format(sc->hw_softc, &vs->vs_format);
1097 if (err != 0)
1098 return err;
1099 }
1100 return 0;
1101 }
1102
1103
1104 int
1105 videoclose(dev_t dev, int flags, int ifmt, struct lwp *l)
1106 {
1107 struct video_softc *sc;
1108 const struct video_hw_if *hw;
1109
1110 sc = device_private(device_lookup(&video_cd, VIDEOUNIT(dev)));
1111 if (sc == NULL)
1112 return ENXIO;
1113
1114 DPRINTF(("videoclose: sc=%p\n", sc));
1115
1116 hw = sc->hw_if;
1117 if (hw == NULL)
1118 return ENXIO;
1119
1120 device_active(sc->sc_dev, DVA_SYSTEM);
1121
1122 video_stream_off(sc, sc->sc_stream_in.vs_type);
1123
1124 /* ignore error */
1125 if (hw->close != NULL)
1126 hw->close(sc->hw_softc);
1127
1128 video_stream_teardown_bufs(&sc->sc_stream_in);
1129
1130 sc->sc_open = 0;
1131 sc->sc_opencnt--;
1132
1133 return 0;
1134 }
1135
1136
1137 int
1138 videoread(dev_t dev, struct uio *uio, int ioflag)
1139 {
1140 struct video_softc *sc;
1141 struct video_stream *vs;
1142 struct video_buffer *vb;
1143 struct scatter_io sio;
1144 int err;
1145 size_t len;
1146 off_t offset;
1147
1148 sc = device_private(device_lookup(&video_cd, VIDEOUNIT(dev)));
1149 if (sc == NULL)
1150 return ENXIO;
1151
1152 if (sc->sc_dying)
1153 return EIO;
1154
1155 vs = &sc->sc_stream_in;
1156
1157 /* userspace has chosen read() method */
1158 if (vs->vs_method == VIDEO_STREAM_METHOD_NONE) {
1159 err = video_stream_setup_bufs(vs,
1160 VIDEO_STREAM_METHOD_READ,
1161 VIDEO_NUM_BUFS);
1162 if (err != 0)
1163 return err;
1164
1165 err = video_stream_on(sc, vs->vs_type);
1166 if (err != 0)
1167 return err;
1168 } else if (vs->vs_method != VIDEO_STREAM_METHOD_READ) {
1169 return EBUSY;
1170 }
1171
1172 mutex_enter(&vs->vs_lock);
1173
1174 retry:
1175 if (SIMPLEQ_EMPTY(&vs->vs_egress)) {
1176 if (vs->vs_flags & O_NONBLOCK) {
1177 mutex_exit(&vs->vs_lock);
1178 return EAGAIN;
1179 }
1180
1181 /* Block until we have a sample */
1182 while (SIMPLEQ_EMPTY(&vs->vs_egress)) {
1183 err = cv_wait_sig(&vs->vs_sample_cv,
1184 &vs->vs_lock);
1185 if (err != 0) {
1186 mutex_exit(&vs->vs_lock);
1187 return EINTR;
1188 }
1189 }
1190
1191 vb = SIMPLEQ_FIRST(&vs->vs_egress);
1192 } else {
1193 vb = SIMPLEQ_FIRST(&vs->vs_egress);
1194 }
1195
1196 /* Oops, empty sample buffer. */
1197 if (vb->vb_buf->bytesused == 0) {
1198 vb = video_stream_dequeue(vs);
1199 video_stream_enqueue(vs, vb);
1200 vs->vs_bytesread = 0;
1201 goto retry;
1202 }
1203
1204 mutex_exit(&vs->vs_lock);
1205
1206 len = min(uio->uio_resid, vb->vb_buf->bytesused - vs->vs_bytesread);
1207 offset = vb->vb_buf->m.offset + vs->vs_bytesread;
1208
1209 if (scatter_io_init(&vs->vs_data, offset, len, &sio)) {
1210 err = scatter_io_uiomove(&sio, uio);
1211 if (err == EFAULT)
1212 return EFAULT;
1213 vs->vs_bytesread += (len - sio.sio_resid);
1214 } else {
1215 DPRINTF(("video: invalid read\n"));
1216 }
1217
1218 /* Move the sample to the ingress queue if everything has
1219 * been read */
1220 if (vs->vs_bytesread >= vb->vb_buf->bytesused) {
1221 mutex_enter(&vs->vs_lock);
1222 vb = video_stream_dequeue(vs);
1223 video_stream_enqueue(vs, vb);
1224 mutex_exit(&vs->vs_lock);
1225
1226 vs->vs_bytesread = 0;
1227 }
1228
1229 return 0;
1230 }
1231
1232
1233 int
1234 videowrite(dev_t dev, struct uio *uio, int ioflag)
1235 {
1236 return ENXIO;
1237 }
1238
1239
1240 int
1241 videoioctl(dev_t dev, u_long cmd, void *data, int flag, struct lwp *l)
1242 {
1243 struct video_softc *sc;
1244 const struct video_hw_if *hw;
1245 struct v4l2_capability *cap;
1246 struct v4l2_fmtdesc *fmtdesc;
1247 struct v4l2_format *fmt;
1248 struct v4l2_standard *std;
1249 struct v4l2_input *input;
1250 struct v4l2_control *control;
1251 struct v4l2_queryctrl *query;
1252 struct v4l2_requestbuffers *reqbufs;
1253 struct v4l2_buffer *buf;
1254 v4l2_std_id *stdid;
1255 enum v4l2_buf_type *typep;
1256 int *ip;
1257
1258 sc = device_private(device_lookup(&video_cd, VIDEOUNIT(dev)));
1259
1260 if (sc->sc_dying)
1261 return EIO;
1262
1263 hw = sc->hw_if;
1264 if (hw == NULL)
1265 return ENXIO;
1266
1267 switch (cmd) {
1268 case VIDIOC_QUERYCAP:
1269 cap = data;
1270 memset(cap, 0, sizeof(*cap));
1271 strlcpy(cap->driver, device_xname(sc->hw_dev),
1272 sizeof(cap->driver));
1273 strlcpy(cap->card, hw->get_devname(sc->hw_softc),
1274 sizeof(cap->card));
1275 /* FIXME: bus_info is wrongly hardcoded to USB */
1276 strlcpy(cap->bus_info, "USB", sizeof(cap->bus_info));
1277 cap->version = VIDEO_DRIVER_VERSION;
1278 cap->capabilities = 0;
1279 if (hw->start_transfer != NULL && hw->stop_transfer != NULL)
1280 cap->capabilities |= V4L2_CAP_VIDEO_CAPTURE |
1281 V4L2_CAP_READWRITE | V4L2_CAP_STREAMING;
1282 return 0;
1283 case VIDIOC_ENUM_FMT:
1284 /* TODO: for now, just enumerate one default format */
1285 fmtdesc = data;
1286 if (fmtdesc->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
1287 return EINVAL;
1288 return video_enum_format(sc, fmtdesc);
1289 case VIDIOC_G_FMT:
1290 fmt = data;
1291 return (video_get_format(sc, fmt));
1292 case VIDIOC_S_FMT:
1293 fmt = data;
1294 if ((flag & FWRITE) == 0)
1295 return EPERM;
1296 return video_set_format(sc, fmt);
1297 case VIDIOC_TRY_FMT:
1298 fmt = data;
1299 return (video_try_format(sc, fmt));
1300 case VIDIOC_ENUMSTD:
1301 /* TODO: implement properly */
1302 std = data;
1303 if (std->index != 0)
1304 return EINVAL;
1305 std->id = V4L2_STD_UNKNOWN;
1306 strlcpy(std->name, "webcam", sizeof(std->name));
1307 return 0;
1308 case VIDIOC_G_STD:
1309 /* TODO: implement properly */
1310 stdid = data;
1311 *stdid = V4L2_STD_UNKNOWN;
1312 return 0;
1313 case VIDIOC_S_STD:
1314 /* TODO: implement properly */
1315 stdid = data;
1316 if (*stdid != V4L2_STD_UNKNOWN)
1317 return EINVAL;
1318 return 0;
1319 case VIDIOC_ENUMINPUT:
1320 /* TODO: implement properly */
1321 input = data;
1322 if (input->index != 0)
1323 return EINVAL;
1324 memset(input, 0, sizeof(*input));
1325 input->index = 0;
1326 strlcpy(input->name, "Camera", sizeof(input->name));
1327 input->type = V4L2_INPUT_TYPE_CAMERA;
1328 return 0;
1329 case VIDIOC_G_INPUT:
1330 /* TODO: implement properly */
1331 ip = data;
1332 *ip = 0;
1333 return 0;
1334 case VIDIOC_S_INPUT:
1335 /* TODO: implement properly */
1336 ip = data;
1337 if (*ip != 0)
1338 return EINVAL;
1339 return 0;
1340 case VIDIOC_QUERYCTRL:
1341 query = data;
1342 return (video_query_control(sc, query));
1343 case VIDIOC_G_CTRL:
1344 control = data;
1345 return (video_get_control(sc, control));
1346 case VIDIOC_S_CTRL:
1347 control = data;
1348 if ((flag & FWRITE) == 0)
1349 return EPERM;
1350 return (video_set_control(sc, control));
1351 case VIDIOC_REQBUFS:
1352 reqbufs = data;
1353 return (video_request_bufs(sc, reqbufs));
1354 case VIDIOC_QUERYBUF:
1355 buf = data;
1356 return video_query_buf(sc, buf);
1357 case VIDIOC_QBUF:
1358 buf = data;
1359 return video_queue_buf(sc, buf);
1360 break;
1361 case VIDIOC_DQBUF:
1362 buf = data;
1363 return video_dequeue_buf(sc, buf);
1364 break;
1365 case VIDIOC_STREAMON:
1366 typep = data;
1367 return video_stream_on(sc, *typep);
1368 case VIDIOC_STREAMOFF:
1369 typep = data;
1370 return video_stream_off(sc, *typep);
1371 default:
1372 DPRINTF(("videoioctl: invalid cmd %s (%lx)\n",
1373 video_ioctl_str(cmd), cmd));
1374 return EINVAL;
1375 }
1376 }
1377
1378 #ifdef VIDEO_DEBUG
1379 static const char *
1380 video_ioctl_str(u_long cmd)
1381 {
1382 const char *str;
1383
1384 switch (cmd) {
1385 case VIDIOC_QUERYCAP:
1386 str = "VIDIOC_QUERYCAP";
1387 break;
1388 case VIDIOC_RESERVED:
1389 str = "VIDIOC_RESERVED";
1390 break;
1391 case VIDIOC_ENUM_FMT:
1392 str = "VIDIOC_ENUM_FMT";
1393 break;
1394 case VIDIOC_G_FMT:
1395 str = "VIDIOC_G_FMT";
1396 break;
1397 case VIDIOC_S_FMT:
1398 str = "VIDIOC_S_FMT";
1399 break;
1400 /* 6 and 7 are VIDIOC_[SG]_COMP, which are unsupported */
1401 case VIDIOC_REQBUFS:
1402 str = "VIDIOC_REQBUFS";
1403 break;
1404 case VIDIOC_QUERYBUF:
1405 str = "VIDIOC_QUERYBUF";
1406 break;
1407 case VIDIOC_G_FBUF:
1408 str = "VIDIOC_G_FBUF";
1409 break;
1410 case VIDIOC_S_FBUF:
1411 str = "VIDIOC_S_FBUF";
1412 break;
1413 case VIDIOC_OVERLAY:
1414 str = "VIDIOC_OVERLAY";
1415 break;
1416 case VIDIOC_QBUF:
1417 str = "VIDIOC_QBUF";
1418 break;
1419 case VIDIOC_DQBUF:
1420 str = "VIDIOC_DQBUF";
1421 break;
1422 case VIDIOC_STREAMON:
1423 str = "VIDIOC_STREAMON";
1424 break;
1425 case VIDIOC_STREAMOFF:
1426 str = "VIDIOC_STREAMOFF";
1427 break;
1428 case VIDIOC_G_PARM:
1429 str = "VIDIOC_G_PARAM";
1430 break;
1431 case VIDIOC_S_PARM:
1432 str = "VIDIOC_S_PARAM";
1433 break;
1434 case VIDIOC_G_STD:
1435 str = "VIDIOC_G_STD";
1436 break;
1437 case VIDIOC_S_STD:
1438 str = "VIDIOC_S_STD";
1439 break;
1440 case VIDIOC_ENUMSTD:
1441 str = "VIDIOC_ENUMSTD";
1442 break;
1443 case VIDIOC_ENUMINPUT:
1444 str = "VIDIOC_ENUMINPUT";
1445 break;
1446 case VIDIOC_G_CTRL:
1447 str = "VIDIOC_G_CTRL";
1448 break;
1449 case VIDIOC_S_CTRL:
1450 str = "VIDIOC_S_CTRL";
1451 break;
1452 case VIDIOC_G_TUNER:
1453 str = "VIDIOC_G_TUNER";
1454 break;
1455 case VIDIOC_S_TUNER:
1456 str = "VIDIOC_S_TUNER";
1457 break;
1458 case VIDIOC_G_AUDIO:
1459 str = "VIDIOC_G_AUDIO";
1460 break;
1461 case VIDIOC_S_AUDIO:
1462 str = "VIDIOC_S_AUDIO";
1463 break;
1464 case VIDIOC_QUERYCTRL:
1465 str = "VIDIOC_QUERYCTRL";
1466 break;
1467 case VIDIOC_QUERYMENU:
1468 str = "VIDIOC_QUERYMENU";
1469 break;
1470 case VIDIOC_G_INPUT:
1471 str = "VIDIOC_G_INPUT";
1472 break;
1473 case VIDIOC_S_INPUT:
1474 str = "VIDIOC_S_INPUT";
1475 break;
1476 case VIDIOC_G_OUTPUT:
1477 str = "VIDIOC_G_OUTPUT";
1478 break;
1479 case VIDIOC_S_OUTPUT:
1480 str = "VIDIOC_S_OUTPUT";
1481 break;
1482 case VIDIOC_ENUMOUTPUT:
1483 str = "VIDIOC_ENUMOUTPUT";
1484 break;
1485 case VIDIOC_G_AUDOUT:
1486 str = "VIDIOC_G_AUDOUT";
1487 break;
1488 case VIDIOC_S_AUDOUT:
1489 str = "VIDIOC_S_AUDOUT";
1490 break;
1491 case VIDIOC_G_MODULATOR:
1492 str = "VIDIOC_G_MODULATOR";
1493 break;
1494 case VIDIOC_S_MODULATOR:
1495 str = "VIDIOC_S_MODULATOR";
1496 break;
1497 case VIDIOC_G_FREQUENCY:
1498 str = "VIDIOC_G_FREQUENCY";
1499 break;
1500 case VIDIOC_S_FREQUENCY:
1501 str = "VIDIOC_S_FREQUENCY";
1502 break;
1503 case VIDIOC_CROPCAP:
1504 str = "VIDIOC_CROPCAP";
1505 break;
1506 case VIDIOC_G_CROP:
1507 str = "VIDIOC_G_CROP";
1508 break;
1509 case VIDIOC_S_CROP:
1510 str = "VIDIOC_S_CROP";
1511 break;
1512 case VIDIOC_G_JPEGCOMP:
1513 str = "VIDIOC_G_JPEGCOMP";
1514 break;
1515 case VIDIOC_S_JPEGCOMP:
1516 str = "VIDIOC_S_JPEGCOMP";
1517 break;
1518 case VIDIOC_QUERYSTD:
1519 str = "VIDIOC_QUERYSTD";
1520 break;
1521 case VIDIOC_TRY_FMT:
1522 str = "VIDIOC_TRY_FMT";
1523 break;
1524 case VIDIOC_ENUMAUDIO:
1525 str = "VIDIOC_ENUMAUDIO";
1526 break;
1527 case VIDIOC_ENUMAUDOUT:
1528 str = "VIDIOC_ENUMAUDOUT";
1529 break;
1530 case VIDIOC_G_PRIORITY:
1531 str = "VIDIOC_G_PRIORITY";
1532 break;
1533 case VIDIOC_S_PRIORITY:
1534 str = "VIDIOC_S_PRIORITY";
1535 break;
1536 default:
1537 str = "unknown";
1538 break;
1539 }
1540 return str;
1541 }
1542 #endif
1543
1544
1545 int
1546 videopoll(dev_t dev, int events, struct lwp *l)
1547 {
1548 struct video_softc *sc;
1549 struct video_stream *vs;
1550 int err, revents = 0;
1551
1552 sc = device_private(device_lookup(&video_cd, VIDEOUNIT(dev)));
1553 vs = &sc->sc_stream_in;
1554
1555 if (sc->sc_dying)
1556 return (POLLHUP);
1557
1558 /* userspace has chosen read() method */
1559 if (vs->vs_method == VIDEO_STREAM_METHOD_NONE) {
1560 err = video_stream_setup_bufs(vs,
1561 VIDEO_STREAM_METHOD_READ,
1562 VIDEO_NUM_BUFS);
1563 if (err != 0)
1564 return POLLERR;
1565
1566 err = video_stream_on(sc, vs->vs_type);
1567 if (err != 0)
1568 return POLLERR;
1569 }
1570
1571 mutex_enter(&vs->vs_lock);
1572 if (!SIMPLEQ_EMPTY(&sc->sc_stream_in.vs_egress))
1573 revents |= events & (POLLIN | POLLRDNORM);
1574 else
1575 selrecord(l, &vs->vs_sel);
1576 mutex_exit(&vs->vs_lock);
1577
1578 return (revents);
1579 }
1580
1581
1582 paddr_t
1583 videommap(dev_t dev, off_t off, int prot)
1584 {
1585 struct video_softc *sc;
1586 struct video_stream *vs;
1587 /* paddr_t pa; */
1588
1589 sc = device_lookup_private(&video_cd, VIDEOUNIT(dev));
1590 if (sc->sc_dying)
1591 return -1;
1592
1593 vs = &sc->sc_stream_in;
1594
1595 return scatter_buf_map(&vs->vs_data, off);
1596 }
1597
1598
1599 /* Allocates buffers and initizlizes some fields. The format field
1600 * must already have been initialized. */
1601 void
1602 video_stream_init(struct video_stream *vs)
1603 {
1604 vs->vs_method = VIDEO_STREAM_METHOD_NONE;
1605 vs->vs_flags = 0;
1606 vs->vs_frameno = -1;
1607 vs->vs_sequence = 0;
1608 vs->vs_type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1609 vs->vs_nbufs = 0;
1610 vs->vs_buf = NULL;
1611 vs->vs_streaming = false;
1612
1613 memset(&vs->vs_format, 0, sizeof(vs->vs_format));
1614
1615 SIMPLEQ_INIT(&vs->vs_ingress);
1616 SIMPLEQ_INIT(&vs->vs_egress);
1617
1618 mutex_init(&vs->vs_lock, MUTEX_DEFAULT, IPL_NONE);
1619 cv_init(&vs->vs_sample_cv, "video");
1620 selinit(&vs->vs_sel);
1621
1622 scatter_buf_init(&vs->vs_data);
1623 }
1624
1625 void
1626 video_stream_fini(struct video_stream *vs)
1627 {
1628 /* Sample data in queues has already been freed */
1629 /* while (SIMPLEQ_FIRST(&vs->vs_ingress) != NULL)
1630 SIMPLEQ_REMOVE_HEAD(&vs->vs_ingress, entries);
1631 while (SIMPLEQ_FIRST(&vs->vs_egress) != NULL)
1632 SIMPLEQ_REMOVE_HEAD(&vs->vs_egress, entries); */
1633
1634 mutex_destroy(&vs->vs_lock);
1635 cv_destroy(&vs->vs_sample_cv);
1636 seldestroy(&vs->vs_sel);
1637
1638 scatter_buf_destroy(&vs->vs_data);
1639 }
1640
1641 static int
1642 video_stream_setup_bufs(struct video_stream *vs,
1643 enum video_stream_method method,
1644 uint8_t nbufs)
1645 {
1646 int i, err;
1647
1648 mutex_enter(&vs->vs_lock);
1649
1650 /* Ensure that all allocated buffers are queued and not under
1651 * userspace control. */
1652 for (i = 0; i < vs->vs_nbufs; ++i) {
1653 if (!(vs->vs_buf[i]->vb_buf->flags & V4L2_BUF_FLAG_QUEUED)) {
1654 mutex_exit(&vs->vs_lock);
1655 return EBUSY;
1656 }
1657 }
1658
1659 /* Allocate the buffers */
1660 err = video_stream_realloc_bufs(vs, nbufs);
1661 if (err != 0) {
1662 mutex_exit(&vs->vs_lock);
1663 return err;
1664 }
1665
1666 /* Queue up buffers for read method. Other methods are queued
1667 * by VIDIOC_QBUF ioctl. */
1668 if (method == VIDEO_STREAM_METHOD_READ) {
1669 for (i = 0; i < nbufs; ++i)
1670 if (!(vs->vs_buf[i]->vb_buf->flags & V4L2_BUF_FLAG_QUEUED))
1671 video_stream_enqueue(vs, vs->vs_buf[i]);
1672 }
1673
1674 vs->vs_method = method;
1675 mutex_exit(&vs->vs_lock);
1676
1677 return 0;
1678 }
1679
1680 /* Free all buffer memory in preparation for close(). This should
1681 * free buffers regardless of errors. Use video_stream_setup_bufs if
1682 * you need to check for errors. Streaming should be off before
1683 * calling this function. */
1684 static void
1685 video_stream_teardown_bufs(struct video_stream *vs)
1686 {
1687 int err;
1688
1689 mutex_enter(&vs->vs_lock);
1690
1691 if (vs->vs_streaming) {
1692 DPRINTF(("video_stream_teardown_bufs: "
1693 "tearing down bufs while streaming\n"));
1694 }
1695
1696 /* dequeue all buffers */
1697 while (SIMPLEQ_FIRST(&vs->vs_ingress) != NULL)
1698 SIMPLEQ_REMOVE_HEAD(&vs->vs_ingress, entries);
1699 while (SIMPLEQ_FIRST(&vs->vs_egress) != NULL)
1700 SIMPLEQ_REMOVE_HEAD(&vs->vs_egress, entries);
1701
1702 err = video_stream_free_bufs(vs);
1703 if (err != 0) {
1704 DPRINTF(("video_stream_teardown_bufs: "
1705 "error releasing buffers: %d\n",
1706 err));
1707 }
1708 vs->vs_method = VIDEO_STREAM_METHOD_NONE;
1709
1710 mutex_exit(&vs->vs_lock);
1711 }
1712
1713 static struct video_buffer *
1714 video_buffer_alloc(void)
1715 {
1716 struct video_buffer *vb;
1717
1718 vb = kmem_alloc(sizeof(*vb), KM_SLEEP);
1719 if (vb == NULL)
1720 return NULL;
1721
1722 vb->vb_buf = kmem_alloc(sizeof(*vb->vb_buf), KM_SLEEP);
1723 if (vb->vb_buf == NULL) {
1724 kmem_free(vb, sizeof(*vb));
1725 return NULL;
1726 }
1727
1728 return vb;
1729 }
1730
1731 static void
1732 video_buffer_free(struct video_buffer *vb)
1733 {
1734 kmem_free(vb->vb_buf, sizeof(*vb->vb_buf));
1735 vb->vb_buf = NULL;
1736 kmem_free(vb, sizeof(*vb));
1737 }
1738
1739 /* TODO: for userptr method
1740 struct video_buffer *
1741 video_buf_alloc_with_ubuf(struct v4l2_buffer *buf)
1742 {
1743 }
1744
1745 void
1746 video_buffer_free_with_ubuf(struct video_buffer *vb)
1747 {
1748 }
1749 */
1750
1751 static int
1752 video_stream_realloc_bufs(struct video_stream *vs, uint8_t nbufs)
1753 {
1754 int i, err;
1755 uint8_t minnbufs, oldnbufs;
1756 size_t size;
1757 off_t offset;
1758 struct video_buffer **oldbuf;
1759 struct v4l2_buffer *buf;
1760
1761 size = vs->vs_format.sample_size * nbufs;
1762 err = scatter_buf_set_size(&vs->vs_data, size);
1763 if (err != 0)
1764 return err;
1765
1766 oldnbufs = vs->vs_nbufs;
1767 oldbuf = vs->vs_buf;
1768
1769 vs->vs_nbufs = nbufs;
1770 if (nbufs > 0) {
1771 vs->vs_buf =
1772 kmem_alloc(sizeof(struct video_buffer *) * nbufs, KM_SLEEP);
1773 if (vs->vs_buf == NULL) {
1774 vs->vs_nbufs = oldnbufs;
1775 vs->vs_buf = oldbuf;
1776
1777 return ENOMEM;
1778 }
1779 } else {
1780 vs->vs_buf = NULL;
1781 }
1782
1783 minnbufs = min(vs->vs_nbufs, oldnbufs);
1784 /* copy any bufs that will be reused */
1785 for (i = 0; i < minnbufs; ++i)
1786 vs->vs_buf[i] = oldbuf[i];
1787 /* allocate any necessary new bufs */
1788 for (; i < vs->vs_nbufs; ++i)
1789 vs->vs_buf[i] = video_buffer_alloc();
1790 /* free any bufs no longer used */
1791 for (; i < oldnbufs; ++i) {
1792 video_buffer_free(oldbuf[i]);
1793 oldbuf[i] = NULL;
1794 }
1795
1796 /* Free old buffer metadata */
1797 if (oldbuf != NULL)
1798 kmem_free(oldbuf, sizeof(struct video_buffer *) * oldnbufs);
1799
1800 /* initialize bufs */
1801 offset = 0;
1802 for (i = 0; i < vs->vs_nbufs; ++i) {
1803 buf = vs->vs_buf[i]->vb_buf;
1804 buf->index = i;
1805 buf->type = vs->vs_type;
1806 buf->bytesused = 0;
1807 buf->flags = 0;
1808 buf->field = 0;
1809 buf->sequence = 0;
1810 buf->memory = V4L2_MEMORY_MMAP;
1811 buf->m.offset = offset;
1812 buf->length = vs->vs_format.sample_size;
1813 buf->input = 0;
1814 buf->reserved = 0;
1815
1816 offset += buf->length;
1817 }
1818
1819 return 0;
1820 }
1821
1822 /* Accepts a video_sample into the ingress queue. Caller must hold
1823 * the stream lock. */
1824 void
1825 video_stream_enqueue(struct video_stream *vs, struct video_buffer *vb)
1826 {
1827 if (vb->vb_buf->flags & V4L2_BUF_FLAG_QUEUED) {
1828 DPRINTF(("video_stream_enqueue: sample already queued\n"));
1829 return;
1830 }
1831
1832 vb->vb_buf->flags |= V4L2_BUF_FLAG_QUEUED;
1833 vb->vb_buf->flags &= ~V4L2_BUF_FLAG_DONE;
1834
1835 vb->vb_buf->bytesused = 0;
1836
1837 SIMPLEQ_INSERT_TAIL(&vs->vs_ingress, vb, entries);
1838 }
1839
1840
1841 /* Removes the head of the egress queue for use by userspace. Caller
1842 * must hold the stream lock. */
1843 struct video_buffer *
1844 video_stream_dequeue(struct video_stream *vs)
1845 {
1846 struct video_buffer *vb;
1847
1848 if (!SIMPLEQ_EMPTY(&vs->vs_egress)) {
1849 vb = SIMPLEQ_FIRST(&vs->vs_egress);
1850 SIMPLEQ_REMOVE_HEAD(&vs->vs_egress, entries);
1851 vb->vb_buf->flags &= ~V4L2_BUF_FLAG_QUEUED;
1852 vb->vb_buf->flags |= V4L2_BUF_FLAG_DONE;
1853 return vb;
1854 } else {
1855 return NULL;
1856 }
1857 }
1858
1859
1860 /*
1861 * write payload data to the appropriate video sample, possibly moving
1862 * the sample from ingress to egress queues
1863 */
1864 void
1865 video_stream_write(struct video_stream *vs,
1866 const struct video_payload *payload)
1867 {
1868 struct video_buffer *vb;
1869 struct v4l2_buffer *buf;
1870 struct scatter_io sio;
1871
1872 mutex_enter(&vs->vs_lock);
1873
1874 /* change of frameno implies end of current frame */
1875 if (vs->vs_frameno > 0 && vs->vs_frameno != payload->frameno)
1876 video_stream_sample_done(vs);
1877
1878 if (vs->vs_drop || SIMPLEQ_EMPTY(&vs->vs_ingress)) {
1879 /* DPRINTF(("video_stream_write: dropping sample %d\n",
1880 vs->vs_sequence)); */
1881 vs->vs_drop = true;
1882 } else if (payload->size > 0) {
1883 vb = SIMPLEQ_FIRST(&vs->vs_ingress);
1884 buf = vb->vb_buf;
1885 if (payload->size > buf->length - buf->bytesused) {
1886 DPRINTF(("video_stream_write: "
1887 "payload would overflow\n"));
1888 } else if (scatter_io_init(&vs->vs_data,
1889 buf->m.offset + buf->bytesused,
1890 payload->size,
1891 &sio))
1892 {
1893 scatter_io_copyin(&sio, payload->data);
1894 buf->bytesused += (payload->size - sio.sio_resid);
1895 } else {
1896 DPRINTF(("video_stream_write: failed to init scatter io "
1897 "vb=%p buf=%p "
1898 "buf->m.offset=%d buf->bytesused=%zu "
1899 "payload->size=%zu\n",
1900 vb, buf,
1901 buf->m.offset, buf->bytesused, payload->size));
1902 }
1903 }
1904
1905 /* if the payload marks it, we can do sample_done() early */
1906 if (payload->end_of_frame)
1907 video_stream_sample_done(vs);
1908
1909 mutex_exit(&vs->vs_lock);
1910 }
1911
1912
1913 /* Moves the head of the ingress queue to the tail of the egress
1914 * queue, or resets drop status if we were dropping this sample.
1915 * Caller should hold the stream queue lock. */
1916 void
1917 video_stream_sample_done(struct video_stream *vs)
1918 {
1919 struct video_buffer *vb;
1920
1921 if (vs->vs_drop) {
1922 vs->vs_drop = false;
1923 } else if (!SIMPLEQ_EMPTY(&vs->vs_ingress)) {
1924 vb = SIMPLEQ_FIRST(&vs->vs_ingress);
1925 vb->vb_buf->sequence = vs->vs_sequence;
1926 SIMPLEQ_REMOVE_HEAD(&vs->vs_ingress, entries);
1927
1928 SIMPLEQ_INSERT_TAIL(&vs->vs_egress, vb, entries);
1929 cv_signal(&vs->vs_sample_cv);
1930 selnotify(&vs->vs_sel, 0, 0);
1931 } else {
1932 DPRINTF(("video_stream_sample_done: no sample\n"));
1933 }
1934
1935 vs->vs_frameno ^= 1;
1936 vs->vs_sequence++;
1937 }
1938
1939 /* Check if all buffers are queued, i.e. none are under control of
1940 * userspace. */
1941 /*
1942 static bool
1943 video_stream_all_queued(struct video_stream *vs)
1944 {
1945 }
1946 */
1947
1948
1949 static void
1950 scatter_buf_init(struct scatter_buf *sb)
1951 {
1952 sb->sb_pool = pool_cache_init(PAGE_SIZE, 0, 0, 0,
1953 "video", NULL, IPL_VIDEO,
1954 NULL, NULL, NULL);
1955 sb->sb_size = 0;
1956 sb->sb_npages = 0;
1957 sb->sb_page_ary = NULL;
1958 }
1959
1960 static void
1961 scatter_buf_destroy(struct scatter_buf *sb)
1962 {
1963 /* Do we need to return everything to the pool first? */
1964 scatter_buf_set_size(sb, 0);
1965 pool_cache_destroy(sb->sb_pool);
1966 sb->sb_pool = 0;
1967 sb->sb_npages = 0;
1968 sb->sb_page_ary = NULL;
1969 }
1970
1971 /* Increase or decrease the size of the buffer */
1972 static int
1973 scatter_buf_set_size(struct scatter_buf *sb, size_t sz)
1974 {
1975 int i;
1976 size_t npages, minpages, oldnpages;
1977 uint8_t **old_ary;
1978
1979 npages = (sz >> PAGE_SHIFT) + ((sz & PAGE_MASK) > 0);
1980
1981 if (sb->sb_npages == npages) {
1982 return 0;
1983 }
1984
1985 oldnpages = sb->sb_npages;
1986 old_ary = sb->sb_page_ary;
1987
1988 sb->sb_npages = npages;
1989 if (npages > 0) {
1990 sb->sb_page_ary =
1991 kmem_alloc(sizeof(uint8_t *) * npages, KM_SLEEP);
1992 if (sb->sb_page_ary == NULL) {
1993 sb->sb_npages = oldnpages;
1994 sb->sb_page_ary = old_ary;
1995 return ENOMEM;
1996 }
1997 } else {
1998 sb->sb_page_ary = NULL;
1999 }
2000
2001 minpages = min(npages, oldnpages);
2002 /* copy any pages that will be reused */
2003 for (i = 0; i < minpages; ++i)
2004 sb->sb_page_ary[i] = old_ary[i];
2005 /* allocate any new pages */
2006 for (; i < npages; ++i) {
2007 sb->sb_page_ary[i] = pool_cache_get(sb->sb_pool, 0);
2008 /* TODO: does pool_cache_get return NULL on
2009 * ENOMEM? If so, we need to release or note
2010 * the pages with did allocate
2011 * successfully. */
2012 if (sb->sb_page_ary[i] == NULL) {
2013 DPRINTF(("video: pool_cache_get ENOMEM\n"));
2014 return ENOMEM;
2015 }
2016 }
2017 /* return any pages no longer needed */
2018 for (; i < oldnpages; ++i)
2019 pool_cache_put(sb->sb_pool, old_ary[i]);
2020
2021 if (old_ary != NULL)
2022 kmem_free(old_ary, sizeof(uint8_t *) * oldnpages);
2023
2024 sb->sb_size = sb->sb_npages << PAGE_SHIFT;
2025
2026 return 0;
2027 }
2028
2029
2030 static paddr_t
2031 scatter_buf_map(struct scatter_buf *sb, off_t off)
2032 {
2033 size_t pg;
2034 paddr_t pa;
2035
2036 pg = off >> PAGE_SHIFT;
2037
2038 if (pg >= sb->sb_npages)
2039 return -1;
2040 else if (!pmap_extract(pmap_kernel(), (vaddr_t)sb->sb_page_ary[pg], &pa))
2041 return -1;
2042
2043 return atop(pa);
2044 }
2045
2046 /* Initialize data for an io operation on a scatter buffer. Returns
2047 * true if the transfer is valid, or false if out of range. */
2048 static bool
2049 scatter_io_init(struct scatter_buf *sb,
2050 off_t off, size_t len,
2051 struct scatter_io *sio)
2052 {
2053 if ((off + len) > sb->sb_size) {
2054 DPRINTF(("video: scatter_io_init failed: off=%" PRId64
2055 " len=%zu sb->sb_size=%zu\n",
2056 off, len, sb->sb_size));
2057 return false;
2058 }
2059
2060 sio->sio_buf = sb;
2061 sio->sio_offset = off;
2062 sio->sio_resid = len;
2063
2064 return true;
2065 }
2066
2067 /* Store the pointer and size of the next contiguous segment. Returns
2068 * true if the segment is valid, or false if all has been transfered.
2069 * Does not check for overflow. */
2070 static bool
2071 scatter_io_next(struct scatter_io *sio, void **p, size_t *sz)
2072 {
2073 size_t pg, pgo;
2074
2075 if (sio->sio_resid == 0)
2076 return false;
2077
2078 pg = sio->sio_offset >> PAGE_SHIFT;
2079 pgo = sio->sio_offset & PAGE_MASK;
2080
2081 *sz = min(PAGE_SIZE - pgo, sio->sio_resid);
2082 *p = sio->sio_buf->sb_page_ary[pg] + pgo;
2083
2084 sio->sio_offset += *sz;
2085 sio->sio_resid -= *sz;
2086
2087 return true;
2088 }
2089
2090 /* Semi-undo of a failed segment copy. Updates the scatter_io
2091 * struct to the previous values prior to a failed segment copy. */
2092 static void
2093 scatter_io_undo(struct scatter_io *sio, size_t sz)
2094 {
2095 sio->sio_offset -= sz;
2096 sio->sio_resid += sz;
2097 }
2098
2099 /* Copy data from src into the scatter_buf as described by io. */
2100 static void
2101 scatter_io_copyin(struct scatter_io *sio, const void *p)
2102 {
2103 void *dst;
2104 const uint8_t *src = p;
2105 size_t sz;
2106
2107 while(scatter_io_next(sio, &dst, &sz)) {
2108 memcpy(dst, src, sz);
2109 src += sz;
2110 }
2111 }
2112
2113 /* --not used; commented to avoid compiler warnings--
2114 static void
2115 scatter_io_copyout(struct scatter_io *sio, void *p)
2116 {
2117 void *src;
2118 uint8_t *dst = p;
2119 size_t sz;
2120
2121 while(scatter_io_next(sio, &src, &sz)) {
2122 memcpy(dst, src, sz);
2123 dst += sz;
2124 }
2125 }
2126 */
2127
2128 /* Performat a series of uiomove calls on a scatter buf. Returns
2129 * EFAULT if uiomove EFAULTs on the first segment. Otherwise, returns
2130 * an incomplete transfer but with no error. */
2131 static int
2132 scatter_io_uiomove(struct scatter_io *sio, struct uio *uio)
2133 {
2134 void *p;
2135 size_t sz;
2136 bool first = true;
2137 int err;
2138
2139 while(scatter_io_next(sio, &p, &sz)) {
2140 err = uiomove(p, sz, uio);
2141 if (err == EFAULT) {
2142 scatter_io_undo(sio, sz);
2143 if (first)
2144 return EFAULT;
2145 else
2146 return 0;
2147 }
2148 first = false;
2149 }
2150
2151 return 0;
2152 }
2153
2154 #endif /* NVIDEO > 0 */
Cache object: 6f5f19b931c511b05f16a127be2f2ed7
|