video.c revision 1.18
1 /* $NetBSD: video.c,v 1.18 2008/12/23 03:22:29 jmorse Exp $ */ 2 3 /* 4 * Copyright (c) 2008 Patrick Mahoney <pat (at) polycrystal.org> 5 * All rights reserved. 6 * 7 * This code was written by Patrick Mahoney (pat (at) 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.18 2008/12/23 03:22:29 jmorse 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->get_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 err = cv_wait_sig(&vs->vs_sample_cv, 974 &vs->vs_lock); 975 if (err != 0) { 976 mutex_exit(&vs->vs_lock); 977 return EINTR; 978 } 979 } 980 } 981 982 memcpy(buf, vb->vb_buf, sizeof(*buf)); 983 984 mutex_exit(&vs->vs_lock); 985 break; 986 default: 987 aprint_debug_dev(sc->sc_dev, "unknown vs_method %d\n", 988 vs->vs_method); 989 return EINVAL; 990 } 991 992 return 0; 993 } 994 995 static int 996 video_stream_on(struct video_softc *sc, enum v4l2_buf_type type) 997 { 998 int err; 999 struct video_stream *vs = &sc->sc_stream_in; 1000 const struct video_hw_if *hw; 1001 1002 if (vs->vs_streaming) 1003 return 0; 1004 if (type != vs->vs_type) 1005 return EINVAL; 1006 1007 hw = sc->hw_if; 1008 if (hw == NULL) 1009 return ENXIO; 1010 1011 1012 err = hw->start_transfer(sc->hw_softc); 1013 if (err != 0) 1014 return err; 1015 1016 vs->vs_streaming = true; 1017 return 0; 1018 } 1019 1020 static int 1021 video_stream_off(struct video_softc *sc, enum v4l2_buf_type type) 1022 { 1023 int err; 1024 struct video_stream *vs = &sc->sc_stream_in; 1025 const struct video_hw_if *hw; 1026 1027 if (!vs->vs_streaming) 1028 return 0; 1029 if (type != vs->vs_type) 1030 return EINVAL; 1031 1032 hw = sc->hw_if; 1033 if (hw == NULL) 1034 return ENXIO; 1035 1036 err = hw->stop_transfer(sc->hw_softc); 1037 if (err != 0) 1038 return err; 1039 1040 vs->vs_frameno = -1; 1041 vs->vs_sequence = 0; 1042 vs->vs_streaming = false; 1043 1044 return 0; 1045 } 1046 1047 int 1048 videoopen(dev_t dev, int flags, int ifmt, struct lwp *l) 1049 { 1050 struct video_softc *sc; 1051 const struct video_hw_if *hw; 1052 struct video_stream *vs; 1053 int err; 1054 1055 DPRINTF(("videoopen\n")); 1056 1057 sc = device_private(device_lookup(&video_cd, VIDEOUNIT(dev))); 1058 if (sc == NULL) { 1059 DPRINTF(("videoopen: failed to get softc\n")); 1060 return ENXIO; 1061 } 1062 1063 if (sc->sc_dying) { 1064 DPRINTF(("videoopen: dying\n")); 1065 return EIO; 1066 } 1067 1068 sc->sc_stream_in.vs_flags = flags; 1069 1070 DPRINTF(("videoopen: flags=0x%x sc=%p parent=%p\n", 1071 flags, sc, sc->hw_dev)); 1072 1073 hw = sc->hw_if; 1074 if (hw == NULL) 1075 return ENXIO; 1076 1077 device_active(sc->sc_dev, DVA_SYSTEM); 1078 1079 sc->sc_opencnt++; 1080 1081 if (hw->open != NULL) { 1082 err = hw->open(sc->hw_softc, flags); 1083 if (err) 1084 return err; 1085 } 1086 1087 /* set up input stream. TODO: check flags to determine if 1088 * "read" is desired? */ 1089 vs = &sc->sc_stream_in; 1090 1091 if (hw->get_format != NULL) { 1092 err = hw->get_format(sc->hw_softc, &vs->vs_format); 1093 if (err != 0) 1094 return err; 1095 } 1096 return 0; 1097 } 1098 1099 1100 int 1101 videoclose(dev_t dev, int flags, int ifmt, struct lwp *l) 1102 { 1103 struct video_softc *sc; 1104 const struct video_hw_if *hw; 1105 1106 sc = device_private(device_lookup(&video_cd, VIDEOUNIT(dev))); 1107 if (sc == NULL) 1108 return ENXIO; 1109 1110 DPRINTF(("videoclose: sc=%p\n", sc)); 1111 1112 hw = sc->hw_if; 1113 if (hw == NULL) 1114 return ENXIO; 1115 1116 device_active(sc->sc_dev, DVA_SYSTEM); 1117 1118 video_stream_off(sc, sc->sc_stream_in.vs_type); 1119 1120 /* ignore error */ 1121 if (hw->close != NULL) 1122 hw->close(sc->hw_softc); 1123 1124 video_stream_teardown_bufs(&sc->sc_stream_in); 1125 1126 sc->sc_open = 0; 1127 sc->sc_opencnt--; 1128 1129 return 0; 1130 } 1131 1132 1133 int 1134 videoread(dev_t dev, struct uio *uio, int ioflag) 1135 { 1136 struct video_softc *sc; 1137 struct video_stream *vs; 1138 struct video_buffer *vb; 1139 struct scatter_io sio; 1140 int err; 1141 size_t len; 1142 off_t offset; 1143 1144 sc = device_private(device_lookup(&video_cd, VIDEOUNIT(dev))); 1145 if (sc == NULL) 1146 return ENXIO; 1147 1148 if (sc->sc_dying) 1149 return EIO; 1150 1151 vs = &sc->sc_stream_in; 1152 1153 /* userspace has chosen read() method */ 1154 if (vs->vs_method == VIDEO_STREAM_METHOD_NONE) { 1155 err = video_stream_setup_bufs(vs, 1156 VIDEO_STREAM_METHOD_READ, 1157 VIDEO_NUM_BUFS); 1158 if (err != 0) 1159 return err; 1160 1161 err = video_stream_on(sc, vs->vs_type); 1162 if (err != 0) 1163 return err; 1164 } else if (vs->vs_method != VIDEO_STREAM_METHOD_READ) { 1165 return EBUSY; 1166 } 1167 1168 mutex_enter(&vs->vs_lock); 1169 1170 retry: 1171 if (SIMPLEQ_EMPTY(&vs->vs_egress)) { 1172 if (vs->vs_flags & O_NONBLOCK) { 1173 mutex_exit(&vs->vs_lock); 1174 return EAGAIN; 1175 } 1176 1177 /* Block until we have a sample */ 1178 while (SIMPLEQ_EMPTY(&vs->vs_egress)) { 1179 err = cv_wait_sig(&vs->vs_sample_cv, 1180 &vs->vs_lock); 1181 if (err != 0) { 1182 mutex_exit(&vs->vs_lock); 1183 return EINTR; 1184 } 1185 } 1186 1187 vb = SIMPLEQ_FIRST(&vs->vs_egress); 1188 } else { 1189 vb = SIMPLEQ_FIRST(&vs->vs_egress); 1190 } 1191 1192 /* Oops, empty sample buffer. */ 1193 if (vb->vb_buf->bytesused == 0) { 1194 vb = video_stream_dequeue(vs); 1195 video_stream_enqueue(vs, vb); 1196 vs->vs_bytesread = 0; 1197 goto retry; 1198 } 1199 1200 mutex_exit(&vs->vs_lock); 1201 1202 len = min(uio->uio_resid, vb->vb_buf->bytesused - vs->vs_bytesread); 1203 offset = vb->vb_buf->m.offset + vs->vs_bytesread; 1204 1205 if (scatter_io_init(&vs->vs_data, offset, len, &sio)) { 1206 err = scatter_io_uiomove(&sio, uio); 1207 if (err == EFAULT) 1208 return EFAULT; 1209 vs->vs_bytesread += (len - sio.sio_resid); 1210 } else { 1211 DPRINTF(("video: invalid read\n")); 1212 } 1213 1214 /* Move the sample to the ingress queue if everything has 1215 * been read */ 1216 if (vs->vs_bytesread >= vb->vb_buf->bytesused) { 1217 mutex_enter(&vs->vs_lock); 1218 vb = video_stream_dequeue(vs); 1219 video_stream_enqueue(vs, vb); 1220 mutex_exit(&vs->vs_lock); 1221 1222 vs->vs_bytesread = 0; 1223 } 1224 1225 return 0; 1226 } 1227 1228 1229 int 1230 videowrite(dev_t dev, struct uio *uio, int ioflag) 1231 { 1232 return ENXIO; 1233 } 1234 1235 1236 int 1237 videoioctl(dev_t dev, u_long cmd, void *data, int flag, struct lwp *l) 1238 { 1239 struct video_softc *sc; 1240 const struct video_hw_if *hw; 1241 struct v4l2_capability *cap; 1242 struct v4l2_fmtdesc *fmtdesc; 1243 struct v4l2_format *fmt; 1244 struct v4l2_standard *std; 1245 struct v4l2_input *input; 1246 struct v4l2_control *control; 1247 struct v4l2_queryctrl *query; 1248 struct v4l2_requestbuffers *reqbufs; 1249 struct v4l2_buffer *buf; 1250 v4l2_std_id *stdid; 1251 enum v4l2_buf_type *typep; 1252 int *ip; 1253 1254 sc = device_private(device_lookup(&video_cd, VIDEOUNIT(dev))); 1255 1256 if (sc->sc_dying) 1257 return EIO; 1258 1259 hw = sc->hw_if; 1260 if (hw == NULL) 1261 return ENXIO; 1262 1263 switch (cmd) { 1264 case VIDIOC_QUERYCAP: 1265 cap = data; 1266 memset(cap, 0, sizeof(*cap)); 1267 strlcpy(cap->driver, device_xname(sc->hw_dev), 1268 sizeof(cap->driver)); 1269 strlcpy(cap->card, hw->get_devname(sc->hw_softc), 1270 sizeof(cap->card)); 1271 /* FIXME: bus_info is wrongly hardcoded to USB */ 1272 strlcpy(cap->bus_info, "USB", sizeof(cap->bus_info)); 1273 cap->version = VIDEO_DRIVER_VERSION; 1274 cap->capabilities = 0; 1275 if (hw->start_transfer != NULL && hw->stop_transfer != NULL) 1276 cap->capabilities |= V4L2_CAP_VIDEO_CAPTURE | 1277 V4L2_CAP_READWRITE | V4L2_CAP_STREAMING; 1278 return 0; 1279 case VIDIOC_ENUM_FMT: 1280 /* TODO: for now, just enumerate one default format */ 1281 fmtdesc = data; 1282 if (fmtdesc->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) 1283 return EINVAL; 1284 return video_enum_format(sc, fmtdesc); 1285 case VIDIOC_G_FMT: 1286 fmt = data; 1287 return (video_get_format(sc, fmt)); 1288 case VIDIOC_S_FMT: 1289 fmt = data; 1290 if ((flag & FWRITE) == 0) 1291 return EPERM; 1292 return video_set_format(sc, fmt); 1293 case VIDIOC_TRY_FMT: 1294 fmt = data; 1295 return (video_try_format(sc, fmt)); 1296 case VIDIOC_ENUMSTD: 1297 /* TODO: implement properly */ 1298 std = data; 1299 if (std->index != 0) 1300 return EINVAL; 1301 std->id = V4L2_STD_UNKNOWN; 1302 strlcpy(std->name, "webcam", sizeof(std->name)); 1303 return 0; 1304 case VIDIOC_G_STD: 1305 /* TODO: implement properly */ 1306 stdid = data; 1307 *stdid = V4L2_STD_UNKNOWN; 1308 return 0; 1309 case VIDIOC_S_STD: 1310 /* TODO: implement properly */ 1311 stdid = data; 1312 if (*stdid != V4L2_STD_UNKNOWN) 1313 return EINVAL; 1314 return 0; 1315 case VIDIOC_ENUMINPUT: 1316 /* TODO: implement properly */ 1317 input = data; 1318 if (input->index != 0) 1319 return EINVAL; 1320 memset(input, 0, sizeof(*input)); 1321 input->index = 0; 1322 strlcpy(input->name, "Camera", sizeof(input->name)); 1323 input->type = V4L2_INPUT_TYPE_CAMERA; 1324 return 0; 1325 case VIDIOC_G_INPUT: 1326 /* TODO: implement properly */ 1327 ip = data; 1328 *ip = 0; 1329 return 0; 1330 case VIDIOC_S_INPUT: 1331 /* TODO: implement properly */ 1332 ip = data; 1333 if (*ip != 0) 1334 return EINVAL; 1335 return 0; 1336 case VIDIOC_QUERYCTRL: 1337 query = data; 1338 return (video_query_control(sc, query)); 1339 case VIDIOC_G_CTRL: 1340 control = data; 1341 return (video_get_control(sc, control)); 1342 case VIDIOC_S_CTRL: 1343 control = data; 1344 if ((flag & FWRITE) == 0) 1345 return EPERM; 1346 return (video_set_control(sc, control)); 1347 case VIDIOC_REQBUFS: 1348 reqbufs = data; 1349 return (video_request_bufs(sc, reqbufs)); 1350 case VIDIOC_QUERYBUF: 1351 buf = data; 1352 return video_query_buf(sc, buf); 1353 case VIDIOC_QBUF: 1354 buf = data; 1355 return video_queue_buf(sc, buf); 1356 break; 1357 case VIDIOC_DQBUF: 1358 buf = data; 1359 return video_dequeue_buf(sc, buf); 1360 break; 1361 case VIDIOC_STREAMON: 1362 typep = data; 1363 return video_stream_on(sc, *typep); 1364 case VIDIOC_STREAMOFF: 1365 typep = data; 1366 return video_stream_off(sc, *typep); 1367 default: 1368 DPRINTF(("videoioctl: invalid cmd %s (%lx)\n", 1369 video_ioctl_str(cmd), cmd)); 1370 return EINVAL; 1371 } 1372 } 1373 1374 #ifdef VIDEO_DEBUG 1375 static const char * 1376 video_ioctl_str(u_long cmd) 1377 { 1378 const char *str; 1379 1380 switch (cmd) { 1381 case VIDIOC_QUERYCAP: 1382 str = "VIDIOC_QUERYCAP"; 1383 break; 1384 case VIDIOC_RESERVED: 1385 str = "VIDIOC_RESERVED"; 1386 break; 1387 case VIDIOC_ENUM_FMT: 1388 str = "VIDIOC_ENUM_FMT"; 1389 break; 1390 case VIDIOC_G_FMT: 1391 str = "VIDIOC_G_FMT"; 1392 break; 1393 case VIDIOC_S_FMT: 1394 str = "VIDIOC_S_FMT"; 1395 break; 1396 /* 6 and 7 are VIDIOC_[SG]_COMP, which are unsupported */ 1397 case VIDIOC_REQBUFS: 1398 str = "VIDIOC_REQBUFS"; 1399 break; 1400 case VIDIOC_QUERYBUF: 1401 str = "VIDIOC_QUERYBUF"; 1402 break; 1403 case VIDIOC_G_FBUF: 1404 str = "VIDIOC_G_FBUF"; 1405 break; 1406 case VIDIOC_S_FBUF: 1407 str = "VIDIOC_S_FBUF"; 1408 break; 1409 case VIDIOC_OVERLAY: 1410 str = "VIDIOC_OVERLAY"; 1411 break; 1412 case VIDIOC_QBUF: 1413 str = "VIDIOC_QBUF"; 1414 break; 1415 case VIDIOC_DQBUF: 1416 str = "VIDIOC_DQBUF"; 1417 break; 1418 case VIDIOC_STREAMON: 1419 str = "VIDIOC_STREAMON"; 1420 break; 1421 case VIDIOC_STREAMOFF: 1422 str = "VIDIOC_STREAMOFF"; 1423 break; 1424 case VIDIOC_G_PARM: 1425 str = "VIDIOC_G_PARAM"; 1426 break; 1427 case VIDIOC_S_PARM: 1428 str = "VIDIOC_S_PARAM"; 1429 break; 1430 case VIDIOC_G_STD: 1431 str = "VIDIOC_G_STD"; 1432 break; 1433 case VIDIOC_S_STD: 1434 str = "VIDIOC_S_STD"; 1435 break; 1436 case VIDIOC_ENUMSTD: 1437 str = "VIDIOC_ENUMSTD"; 1438 break; 1439 case VIDIOC_ENUMINPUT: 1440 str = "VIDIOC_ENUMINPUT"; 1441 break; 1442 case VIDIOC_G_CTRL: 1443 str = "VIDIOC_G_CTRL"; 1444 break; 1445 case VIDIOC_S_CTRL: 1446 str = "VIDIOC_S_CTRL"; 1447 break; 1448 case VIDIOC_G_TUNER: 1449 str = "VIDIOC_G_TUNER"; 1450 break; 1451 case VIDIOC_S_TUNER: 1452 str = "VIDIOC_S_TUNER"; 1453 break; 1454 case VIDIOC_G_AUDIO: 1455 str = "VIDIOC_G_AUDIO"; 1456 break; 1457 case VIDIOC_S_AUDIO: 1458 str = "VIDIOC_S_AUDIO"; 1459 break; 1460 case VIDIOC_QUERYCTRL: 1461 str = "VIDIOC_QUERYCTRL"; 1462 break; 1463 case VIDIOC_QUERYMENU: 1464 str = "VIDIOC_QUERYMENU"; 1465 break; 1466 case VIDIOC_G_INPUT: 1467 str = "VIDIOC_G_INPUT"; 1468 break; 1469 case VIDIOC_S_INPUT: 1470 str = "VIDIOC_S_INPUT"; 1471 break; 1472 case VIDIOC_G_OUTPUT: 1473 str = "VIDIOC_G_OUTPUT"; 1474 break; 1475 case VIDIOC_S_OUTPUT: 1476 str = "VIDIOC_S_OUTPUT"; 1477 break; 1478 case VIDIOC_ENUMOUTPUT: 1479 str = "VIDIOC_ENUMOUTPUT"; 1480 break; 1481 case VIDIOC_G_AUDOUT: 1482 str = "VIDIOC_G_AUDOUT"; 1483 break; 1484 case VIDIOC_S_AUDOUT: 1485 str = "VIDIOC_S_AUDOUT"; 1486 break; 1487 case VIDIOC_G_MODULATOR: 1488 str = "VIDIOC_G_MODULATOR"; 1489 break; 1490 case VIDIOC_S_MODULATOR: 1491 str = "VIDIOC_S_MODULATOR"; 1492 break; 1493 case VIDIOC_G_FREQUENCY: 1494 str = "VIDIOC_G_FREQUENCY"; 1495 break; 1496 case VIDIOC_S_FREQUENCY: 1497 str = "VIDIOC_S_FREQUENCY"; 1498 break; 1499 case VIDIOC_CROPCAP: 1500 str = "VIDIOC_CROPCAP"; 1501 break; 1502 case VIDIOC_G_CROP: 1503 str = "VIDIOC_G_CROP"; 1504 break; 1505 case VIDIOC_S_CROP: 1506 str = "VIDIOC_S_CROP"; 1507 break; 1508 case VIDIOC_G_JPEGCOMP: 1509 str = "VIDIOC_G_JPEGCOMP"; 1510 break; 1511 case VIDIOC_S_JPEGCOMP: 1512 str = "VIDIOC_S_JPEGCOMP"; 1513 break; 1514 case VIDIOC_QUERYSTD: 1515 str = "VIDIOC_QUERYSTD"; 1516 break; 1517 case VIDIOC_TRY_FMT: 1518 str = "VIDIOC_TRY_FMT"; 1519 break; 1520 case VIDIOC_ENUMAUDIO: 1521 str = "VIDIOC_ENUMAUDIO"; 1522 break; 1523 case VIDIOC_ENUMAUDOUT: 1524 str = "VIDIOC_ENUMAUDOUT"; 1525 break; 1526 case VIDIOC_G_PRIORITY: 1527 str = "VIDIOC_G_PRIORITY"; 1528 break; 1529 case VIDIOC_S_PRIORITY: 1530 str = "VIDIOC_S_PRIORITY"; 1531 break; 1532 default: 1533 str = "unknown"; 1534 break; 1535 } 1536 return str; 1537 } 1538 #endif 1539 1540 1541 int 1542 videopoll(dev_t dev, int events, struct lwp *l) 1543 { 1544 struct video_softc *sc; 1545 struct video_stream *vs; 1546 int err, revents = 0; 1547 1548 sc = device_private(device_lookup(&video_cd, VIDEOUNIT(dev))); 1549 vs = &sc->sc_stream_in; 1550 1551 if (sc->sc_dying) 1552 return (POLLHUP); 1553 1554 /* userspace has chosen read() method */ 1555 if (vs->vs_method == VIDEO_STREAM_METHOD_NONE) { 1556 err = video_stream_setup_bufs(vs, 1557 VIDEO_STREAM_METHOD_READ, 1558 VIDEO_NUM_BUFS); 1559 if (err != 0) 1560 return POLLERR; 1561 1562 err = video_stream_on(sc, vs->vs_type); 1563 if (err != 0) 1564 return POLLERR; 1565 } 1566 1567 if (!SIMPLEQ_EMPTY(&sc->sc_stream_in.vs_egress)) 1568 revents |= events & (POLLIN | POLLRDNORM); 1569 else 1570 selrecord(l, &vs->vs_sel); 1571 1572 return (revents); 1573 } 1574 1575 1576 paddr_t 1577 videommap(dev_t dev, off_t off, int prot) 1578 { 1579 struct video_softc *sc; 1580 struct video_stream *vs; 1581 /* paddr_t pa; */ 1582 1583 sc = device_lookup_private(&video_cd, VIDEOUNIT(dev)); 1584 if (sc->sc_dying) 1585 return -1; 1586 1587 vs = &sc->sc_stream_in; 1588 1589 return scatter_buf_map(&vs->vs_data, off); 1590 } 1591 1592 1593 /* Allocates buffers and initizlizes some fields. The format field 1594 * must already have been initialized. */ 1595 void 1596 video_stream_init(struct video_stream *vs) 1597 { 1598 vs->vs_method = VIDEO_STREAM_METHOD_NONE; 1599 vs->vs_flags = 0; 1600 vs->vs_frameno = -1; 1601 vs->vs_sequence = 0; 1602 vs->vs_type = V4L2_BUF_TYPE_VIDEO_CAPTURE; 1603 vs->vs_nbufs = 0; 1604 vs->vs_buf = NULL; 1605 vs->vs_streaming = false; 1606 1607 memset(&vs->vs_format, 0, sizeof(vs->vs_format)); 1608 1609 SIMPLEQ_INIT(&vs->vs_ingress); 1610 SIMPLEQ_INIT(&vs->vs_egress); 1611 1612 mutex_init(&vs->vs_lock, MUTEX_DEFAULT, IPL_NONE); 1613 cv_init(&vs->vs_sample_cv, "video"); 1614 selinit(&vs->vs_sel); 1615 1616 scatter_buf_init(&vs->vs_data); 1617 } 1618 1619 void 1620 video_stream_fini(struct video_stream *vs) 1621 { 1622 /* Sample data in queues has already been freed */ 1623 /* while (SIMPLEQ_FIRST(&vs->vs_ingress) != NULL) 1624 SIMPLEQ_REMOVE_HEAD(&vs->vs_ingress, entries); 1625 while (SIMPLEQ_FIRST(&vs->vs_egress) != NULL) 1626 SIMPLEQ_REMOVE_HEAD(&vs->vs_egress, entries); */ 1627 1628 mutex_destroy(&vs->vs_lock); 1629 cv_destroy(&vs->vs_sample_cv); 1630 seldestroy(&vs->vs_sel); 1631 1632 scatter_buf_destroy(&vs->vs_data); 1633 } 1634 1635 static int 1636 video_stream_setup_bufs(struct video_stream *vs, 1637 enum video_stream_method method, 1638 uint8_t nbufs) 1639 { 1640 int i, err; 1641 1642 mutex_enter(&vs->vs_lock); 1643 1644 /* Ensure that all allocated buffers are queued and not under 1645 * userspace control. */ 1646 for (i = 0; i < vs->vs_nbufs; ++i) { 1647 if (!(vs->vs_buf[i]->vb_buf->flags | V4L2_BUF_FLAG_QUEUED)) { 1648 mutex_exit(&vs->vs_lock); 1649 return EBUSY; 1650 } 1651 } 1652 1653 /* Allocate the buffers */ 1654 err = video_stream_realloc_bufs(vs, nbufs); 1655 if (err != 0) { 1656 mutex_exit(&vs->vs_lock); 1657 return err; 1658 } 1659 1660 /* Queue up buffers for read method. Other methods are queued 1661 * by VIDIOC_QBUF ioctl. */ 1662 if (method == VIDEO_STREAM_METHOD_READ) { 1663 for (i = 0; i < nbufs; ++i) 1664 if (!(vs->vs_buf[i]->vb_buf->flags & V4L2_BUF_FLAG_QUEUED)) 1665 video_stream_enqueue(vs, vs->vs_buf[i]); 1666 } 1667 1668 vs->vs_method = method; 1669 mutex_exit(&vs->vs_lock); 1670 1671 return 0; 1672 } 1673 1674 /* Free all buffer memory in preparation for close(). This should 1675 * free buffers regardless of errors. Use video_stream_setup_bufs if 1676 * you need to check for errors. Streaming should be off before 1677 * calling this function. */ 1678 static void 1679 video_stream_teardown_bufs(struct video_stream *vs) 1680 { 1681 int err; 1682 1683 mutex_enter(&vs->vs_lock); 1684 1685 if (vs->vs_streaming) { 1686 DPRINTF(("video_stream_teardown_bufs: " 1687 "tearing down bufs while streaming\n")); 1688 } 1689 1690 /* dequeue all buffers */ 1691 while (SIMPLEQ_FIRST(&vs->vs_ingress) != NULL) 1692 SIMPLEQ_REMOVE_HEAD(&vs->vs_ingress, entries); 1693 while (SIMPLEQ_FIRST(&vs->vs_egress) != NULL) 1694 SIMPLEQ_REMOVE_HEAD(&vs->vs_egress, entries); 1695 1696 err = video_stream_free_bufs(vs); 1697 if (err != 0) { 1698 DPRINTF(("video_stream_teardown_bufs: " 1699 "error releasing buffers: %d\n", 1700 err)); 1701 } 1702 vs->vs_method = VIDEO_STREAM_METHOD_NONE; 1703 1704 mutex_exit(&vs->vs_lock); 1705 } 1706 1707 static struct video_buffer * 1708 video_buffer_alloc(void) 1709 { 1710 struct video_buffer *vb; 1711 1712 vb = kmem_alloc(sizeof(*vb), KM_SLEEP); 1713 if (vb == NULL) 1714 return NULL; 1715 1716 vb->vb_buf = kmem_alloc(sizeof(*vb->vb_buf), KM_SLEEP); 1717 if (vb->vb_buf == NULL) { 1718 kmem_free(vb, sizeof(*vb)); 1719 return NULL; 1720 } 1721 1722 return vb; 1723 } 1724 1725 static void 1726 video_buffer_free(struct video_buffer *vb) 1727 { 1728 kmem_free(vb->vb_buf, sizeof(*vb->vb_buf)); 1729 vb->vb_buf = NULL; 1730 kmem_free(vb, sizeof(*vb)); 1731 } 1732 1733 /* TODO: for userptr method 1734 struct video_buffer * 1735 video_buf_alloc_with_ubuf(struct v4l2_buffer *buf) 1736 { 1737 } 1738 1739 void 1740 video_buffer_free_with_ubuf(struct video_buffer *vb) 1741 { 1742 } 1743 */ 1744 1745 static int 1746 video_stream_realloc_bufs(struct video_stream *vs, uint8_t nbufs) 1747 { 1748 int i, err; 1749 uint8_t minnbufs, oldnbufs; 1750 size_t size; 1751 off_t offset; 1752 struct video_buffer **oldbuf; 1753 struct v4l2_buffer *buf; 1754 1755 size = vs->vs_format.sample_size * nbufs; 1756 err = scatter_buf_set_size(&vs->vs_data, size); 1757 if (err != 0) 1758 return err; 1759 1760 oldnbufs = vs->vs_nbufs; 1761 oldbuf = vs->vs_buf; 1762 1763 vs->vs_nbufs = nbufs; 1764 if (nbufs > 0) { 1765 vs->vs_buf = 1766 kmem_alloc(sizeof(struct video_buffer *) * nbufs, KM_SLEEP); 1767 if (vs->vs_buf == NULL) { 1768 vs->vs_nbufs = oldnbufs; 1769 vs->vs_buf = oldbuf; 1770 1771 return ENOMEM; 1772 } 1773 } else { 1774 vs->vs_buf = NULL; 1775 } 1776 1777 minnbufs = min(vs->vs_nbufs, oldnbufs); 1778 /* copy any bufs that will be reused */ 1779 for (i = 0; i < minnbufs; ++i) 1780 vs->vs_buf[i] = oldbuf[i]; 1781 /* allocate any necessary new bufs */ 1782 for (; i < vs->vs_nbufs; ++i) 1783 vs->vs_buf[i] = video_buffer_alloc(); 1784 /* free any bufs no longer used */ 1785 for (; i < oldnbufs; ++i) { 1786 video_buffer_free(oldbuf[i]); 1787 oldbuf[i] = NULL; 1788 } 1789 1790 /* Free old buffer metadata */ 1791 if (oldbuf != NULL) 1792 kmem_free(oldbuf, sizeof(struct video_buffer *) * oldnbufs); 1793 1794 /* initialize bufs */ 1795 offset = 0; 1796 for (i = 0; i < vs->vs_nbufs; ++i) { 1797 buf = vs->vs_buf[i]->vb_buf; 1798 buf->index = i; 1799 buf->type = vs->vs_type; 1800 buf->bytesused = 0; 1801 buf->flags = 0; 1802 buf->field = 0; 1803 buf->sequence = 0; 1804 buf->memory = V4L2_MEMORY_MMAP; 1805 buf->m.offset = offset; 1806 buf->length = vs->vs_format.sample_size; 1807 buf->input = 0; 1808 buf->reserved = 0; 1809 1810 offset += buf->length; 1811 } 1812 1813 return 0; 1814 } 1815 1816 /* Accepts a video_sample into the ingress queue. Caller must hold 1817 * the stream lock. */ 1818 void 1819 video_stream_enqueue(struct video_stream *vs, struct video_buffer *vb) 1820 { 1821 if (vb->vb_buf->flags & V4L2_BUF_FLAG_QUEUED) { 1822 DPRINTF(("video_stream_enqueue: sample already queued\n")); 1823 return; 1824 } 1825 1826 vb->vb_buf->flags |= V4L2_BUF_FLAG_QUEUED; 1827 vb->vb_buf->flags &= ~V4L2_BUF_FLAG_DONE; 1828 1829 vb->vb_buf->bytesused = 0; 1830 1831 SIMPLEQ_INSERT_TAIL(&vs->vs_ingress, vb, entries); 1832 } 1833 1834 1835 /* Removes the head of the egress queue for use by userspace. Caller 1836 * must hold the stream lock. */ 1837 struct video_buffer * 1838 video_stream_dequeue(struct video_stream *vs) 1839 { 1840 struct video_buffer *vb; 1841 1842 if (!SIMPLEQ_EMPTY(&vs->vs_egress)) { 1843 vb = SIMPLEQ_FIRST(&vs->vs_egress); 1844 SIMPLEQ_REMOVE_HEAD(&vs->vs_egress, entries); 1845 vb->vb_buf->flags &= ~V4L2_BUF_FLAG_QUEUED; 1846 vb->vb_buf->flags |= V4L2_BUF_FLAG_DONE; 1847 return vb; 1848 } else { 1849 return NULL; 1850 } 1851 } 1852 1853 1854 /* 1855 * write payload data to the appropriate video sample, possibly moving 1856 * the sample from ingress to egress queues 1857 */ 1858 void 1859 video_stream_write(struct video_stream *vs, 1860 const struct video_payload *payload) 1861 { 1862 struct video_buffer *vb; 1863 struct v4l2_buffer *buf; 1864 struct scatter_io sio; 1865 1866 mutex_enter(&vs->vs_lock); 1867 1868 /* change of frameno implies end of current frame */ 1869 if (vs->vs_frameno > 0 && vs->vs_frameno != payload->frameno) 1870 video_stream_sample_done(vs); 1871 1872 if (vs->vs_drop || SIMPLEQ_EMPTY(&vs->vs_ingress)) { 1873 /* DPRINTF(("video_stream_write: dropping sample %d\n", 1874 vs->vs_sequence)); */ 1875 vs->vs_drop = true; 1876 } else if (payload->size > 0) { 1877 vb = SIMPLEQ_FIRST(&vs->vs_ingress); 1878 buf = vb->vb_buf; 1879 if (payload->size > buf->length - buf->bytesused) { 1880 DPRINTF(("video_stream_write: " 1881 "payload would overflow\n")); 1882 } else if (scatter_io_init(&vs->vs_data, 1883 buf->m.offset + buf->bytesused, 1884 payload->size, 1885 &sio)) 1886 { 1887 scatter_io_copyin(&sio, payload->data); 1888 buf->bytesused += (payload->size - sio.sio_resid); 1889 } else { 1890 DPRINTF(("video_stream_write: failed to init scatter io " 1891 "vb=%p buf=%p " 1892 "buf->m.offset=%d buf->bytesused=%zu " 1893 "payload->size=%zu\n", 1894 vb, buf, 1895 buf->m.offset, buf->bytesused, payload->size)); 1896 } 1897 } 1898 1899 /* if the payload marks it, we can do sample_done() early */ 1900 if (payload->end_of_frame) 1901 video_stream_sample_done(vs); 1902 1903 mutex_exit(&vs->vs_lock); 1904 } 1905 1906 1907 /* Moves the head of the ingress queue to the tail of the egress 1908 * queue, or resets drop status if we were dropping this sample. 1909 * Caller should hold the stream queue lock. */ 1910 void 1911 video_stream_sample_done(struct video_stream *vs) 1912 { 1913 struct video_buffer *vb; 1914 1915 if (vs->vs_drop) { 1916 vs->vs_drop = false; 1917 } else if (!SIMPLEQ_EMPTY(&vs->vs_ingress)) { 1918 vb = SIMPLEQ_FIRST(&vs->vs_ingress); 1919 vb->vb_buf->sequence = vs->vs_sequence; 1920 SIMPLEQ_REMOVE_HEAD(&vs->vs_ingress, entries); 1921 1922 SIMPLEQ_INSERT_TAIL(&vs->vs_egress, vb, entries); 1923 cv_signal(&vs->vs_sample_cv); 1924 selnotify(&vs->vs_sel, 0, 0); 1925 } else { 1926 DPRINTF(("video_stream_sample_done: no sample\n")); 1927 } 1928 1929 vs->vs_frameno ^= 1; 1930 vs->vs_sequence++; 1931 } 1932 1933 /* Check if all buffers are queued, i.e. none are under control of 1934 * userspace. */ 1935 /* 1936 static bool 1937 video_stream_all_queued(struct video_stream *vs) 1938 { 1939 } 1940 */ 1941 1942 1943 static void 1944 scatter_buf_init(struct scatter_buf *sb) 1945 { 1946 sb->sb_pool = pool_cache_init(PAGE_SIZE, 0, 0, 0, 1947 "video", NULL, IPL_VIDEO, 1948 NULL, NULL, NULL); 1949 sb->sb_size = 0; 1950 sb->sb_npages = 0; 1951 sb->sb_page_ary = NULL; 1952 } 1953 1954 static void 1955 scatter_buf_destroy(struct scatter_buf *sb) 1956 { 1957 /* Do we need to return everything to the pool first? */ 1958 scatter_buf_set_size(sb, 0); 1959 pool_cache_destroy(sb->sb_pool); 1960 sb->sb_pool = 0; 1961 sb->sb_npages = 0; 1962 sb->sb_page_ary = NULL; 1963 } 1964 1965 /* Increase or decrease the size of the buffer */ 1966 static int 1967 scatter_buf_set_size(struct scatter_buf *sb, size_t sz) 1968 { 1969 int i; 1970 size_t npages, minpages, oldnpages; 1971 uint8_t **old_ary; 1972 1973 npages = (sz >> PAGE_SHIFT) + ((sz & PAGE_MASK) > 0); 1974 1975 if (sb->sb_npages == npages) { 1976 return 0; 1977 } 1978 1979 oldnpages = sb->sb_npages; 1980 old_ary = sb->sb_page_ary; 1981 1982 sb->sb_npages = npages; 1983 if (npages > 0) { 1984 sb->sb_page_ary = 1985 kmem_alloc(sizeof(uint8_t *) * npages, KM_SLEEP); 1986 if (sb->sb_page_ary == NULL) { 1987 sb->sb_npages = oldnpages; 1988 sb->sb_page_ary = old_ary; 1989 return ENOMEM; 1990 } 1991 } else { 1992 sb->sb_page_ary = NULL; 1993 } 1994 1995 minpages = min(npages, oldnpages); 1996 /* copy any pages that will be reused */ 1997 for (i = 0; i < minpages; ++i) 1998 sb->sb_page_ary[i] = old_ary[i]; 1999 /* allocate any new pages */ 2000 for (; i < npages; ++i) { 2001 sb->sb_page_ary[i] = pool_cache_get(sb->sb_pool, 0); 2002 /* TODO: does pool_cache_get return NULL on 2003 * ENOMEM? If so, we need to release or note 2004 * the pages with did allocate 2005 * successfully. */ 2006 if (sb->sb_page_ary[i] == NULL) { 2007 DPRINTF(("video: pool_cache_get ENOMEM\n")); 2008 return ENOMEM; 2009 } 2010 } 2011 /* return any pages no longer needed */ 2012 for (; i < oldnpages; ++i) 2013 pool_cache_put(sb->sb_pool, old_ary[i]); 2014 2015 if (old_ary != NULL) 2016 kmem_free(old_ary, sizeof(uint8_t *) * oldnpages); 2017 2018 sb->sb_size = sb->sb_npages << PAGE_SHIFT; 2019 2020 return 0; 2021 } 2022 2023 2024 static paddr_t 2025 scatter_buf_map(struct scatter_buf *sb, off_t off) 2026 { 2027 size_t pg; 2028 paddr_t pa; 2029 2030 pg = off >> PAGE_SHIFT; 2031 2032 if (pg >= sb->sb_npages) 2033 return -1; 2034 else if (!pmap_extract(pmap_kernel(), (vaddr_t)sb->sb_page_ary[pg], &pa)) 2035 return -1; 2036 2037 return atop(pa); 2038 } 2039 2040 /* Initialize data for an io operation on a scatter buffer. Returns 2041 * true if the transfer is valid, or false if out of range. */ 2042 static bool 2043 scatter_io_init(struct scatter_buf *sb, 2044 off_t off, size_t len, 2045 struct scatter_io *sio) 2046 { 2047 if ((off + len) > sb->sb_size) { 2048 DPRINTF(("video: scatter_io_init failed: off=%" PRId64 2049 " len=%zu sb->sb_size=%zu\n", 2050 off, len, sb->sb_size)); 2051 return false; 2052 } 2053 2054 sio->sio_buf = sb; 2055 sio->sio_offset = off; 2056 sio->sio_resid = len; 2057 2058 return true; 2059 } 2060 2061 /* Store the pointer and size of the next contiguous segment. Returns 2062 * true if the segment is valid, or false if all has been transfered. 2063 * Does not check for overflow. */ 2064 static bool 2065 scatter_io_next(struct scatter_io *sio, void **p, size_t *sz) 2066 { 2067 size_t pg, pgo; 2068 2069 if (sio->sio_resid == 0) 2070 return false; 2071 2072 pg = sio->sio_offset >> PAGE_SHIFT; 2073 pgo = sio->sio_offset & PAGE_MASK; 2074 2075 *sz = min(PAGE_SIZE - pgo, sio->sio_resid); 2076 *p = sio->sio_buf->sb_page_ary[pg] + pgo; 2077 2078 sio->sio_offset += *sz; 2079 sio->sio_resid -= *sz; 2080 2081 return true; 2082 } 2083 2084 /* Semi-undo of a failed segment copy. Updates the scatter_io 2085 * struct to the previous values prior to a failed segment copy. */ 2086 static void 2087 scatter_io_undo(struct scatter_io *sio, size_t sz) 2088 { 2089 sio->sio_offset -= sz; 2090 sio->sio_resid += sz; 2091 } 2092 2093 /* Copy data from src into the scatter_buf as described by io. */ 2094 static void 2095 scatter_io_copyin(struct scatter_io *sio, const void *p) 2096 { 2097 void *dst; 2098 const uint8_t *src = p; 2099 size_t sz; 2100 2101 while(scatter_io_next(sio, &dst, &sz)) { 2102 memcpy(dst, src, sz); 2103 src += sz; 2104 } 2105 } 2106 2107 /* --not used; commented to avoid compiler warnings-- 2108 static void 2109 scatter_io_copyout(struct scatter_io *sio, void *p) 2110 { 2111 void *src; 2112 uint8_t *dst = p; 2113 size_t sz; 2114 2115 while(scatter_io_next(sio, &src, &sz)) { 2116 memcpy(dst, src, sz); 2117 dst += sz; 2118 } 2119 } 2120 */ 2121 2122 /* Performat a series of uiomove calls on a scatter buf. Returns 2123 * EFAULT if uiomove EFAULTs on the first segment. Otherwise, returns 2124 * an incomplete transfer but with no error. */ 2125 static int 2126 scatter_io_uiomove(struct scatter_io *sio, struct uio *uio) 2127 { 2128 void *p; 2129 size_t sz; 2130 bool first = true; 2131 int err; 2132 2133 while(scatter_io_next(sio, &p, &sz)) { 2134 err = uiomove(p, sz, uio); 2135 if (err == EFAULT) { 2136 scatter_io_undo(sio, sz); 2137 if (first) 2138 return EFAULT; 2139 else 2140 return 0; 2141 } 2142 first = false; 2143 } 2144 2145 return 0; 2146 } 2147 2148 #endif /* NVIDEO > 0 */ 2149
Indexes created Thu Sep 25 16:09:42 GMT 2025