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