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