The Design and Implementation of the FreeBSD Operating System, Second Edition
Now available: The Design and Implementation of the FreeBSD Operating System (Second Edition)


[ source navigation ] [ diff markup ] [ identifier search ] [ freetext search ] [ file search ] [ list types ] [ track identifier ]

FreeBSD/Linux Kernel Cross Reference
sys/dev/video.c

Version: -  FREEBSD  -  FREEBSD-13-STABLE  -  FREEBSD-13-0  -  FREEBSD-12-STABLE  -  FREEBSD-12-0  -  FREEBSD-11-STABLE  -  FREEBSD-11-0  -  FREEBSD-10-STABLE  -  FREEBSD-10-0  -  FREEBSD-9-STABLE  -  FREEBSD-9-0  -  FREEBSD-8-STABLE  -  FREEBSD-8-0  -  FREEBSD-7-STABLE  -  FREEBSD-7-0  -  FREEBSD-6-STABLE  -  FREEBSD-6-0  -  FREEBSD-5-STABLE  -  FREEBSD-5-0  -  FREEBSD-4-STABLE  -  FREEBSD-3-STABLE  -  FREEBSD22  -  l41  -  OPENBSD  -  linux-2.6  -  MK84  -  PLAN9  -  xnu-8792 
SearchContext: -  none  -  3  -  10 

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

Cache object: 6f5f19b931c511b05f16a127be2f2ed7


[ source navigation ] [ diff markup ] [ identifier search ] [ freetext search ] [ file search ] [ list types ] [ track identifier ]


This page is part of the FreeBSD/Linux Linux Kernel Cross-Reference, and was automatically generated using a modified version of the LXR engine.