/* * CDDL HEADER START * * The contents of this file are subject to the terms of the * Common Development and Distribution License (the "License"). * You may not use this file except in compliance with the License. * * You can obtain a copy of the license at usr/src/OPENSOLARIS.LICENSE * or http://www.opensolaris.org/os/licensing. * See the License for the specific language governing permissions * and limitations under the License. * * When distributing Covered Code, include this CDDL HEADER in each * file and include the License file at usr/src/OPENSOLARIS.LICENSE. * If applicable, add the following below this CDDL HEADER, with the * fields enclosed by brackets "[]" replaced with your own identifying * information: Portions Copyright [yyyy] [name of copyright owner] * * CDDL HEADER END */ /* * Copyright 2008 Sun Microsystems, Inc. All rights reserved. * Use is subject to license terms. */ /* * USB video class driver: V4L2 interface implementation. */ #include #include #include #include #include #include static int usbvc_v4l2_set_format(usbvc_state_t *, struct v4l2_format *); static int usbvc_v4l2_get_format(usbvc_state_t *, struct v4l2_format *); static void usbvc_v4l2_query_buf(usbvc_state_t *, usbvc_buf_t *, struct v4l2_buffer *); static int usbvc_v4l2_enqueue_buf(usbvc_state_t *, usbvc_buf_t *, struct v4l2_buffer *); static int usbvc_v4l2_dequeue_buffer(usbvc_state_t *, struct v4l2_buffer *, int); static int usbvc_v4l2_query_ctrl(usbvc_state_t *, struct v4l2_queryctrl *); static int usbvc_v4l2_get_ctrl(usbvc_state_t *, struct v4l2_control *); static int usbvc_v4l2_set_ctrl(usbvc_state_t *, struct v4l2_control *); static int usbvc_v4l2_set_parm(usbvc_state_t *, struct v4l2_streamparm *); static int usbvc_v4l2_get_parm(usbvc_state_t *, struct v4l2_streamparm *); /* Video controls that supported by usbvc driver */ static usbvc_v4l2_ctrl_map_t usbvc_v4l2_ctrls[] = { { "Brightness", PU_BRIGHTNESS_CONTROL, 2, 0, V4L2_CTRL_TYPE_INTEGER }, { "Contrast", PU_CONTRAST_CONTROL, 2, 1, V4L2_CTRL_TYPE_INTEGER }, { "Saturation", PU_SATURATION_CONTROL, 2, 3, V4L2_CTRL_TYPE_INTEGER }, { "Hue", PU_HUE_CONTROL, 2, 2, V4L2_CTRL_TYPE_INTEGER }, { "Gamma", PU_GAMMA_CONTROL, 2, 5, V4L2_CTRL_TYPE_INTEGER } }; /* * V4L2 colorspaces. */ static const uint8_t color_primaries[] = { 0, V4L2_COLORSPACE_SRGB, V4L2_COLORSPACE_470_SYSTEM_M, V4L2_COLORSPACE_470_SYSTEM_BG, V4L2_COLORSPACE_SMPTE170M, V4L2_COLORSPACE_SMPTE240M, }; /* V4L2 ioctls */ int usbvc_v4l2_ioctl(usbvc_state_t *usbvcp, int cmd, intptr_t arg, int mode) { int rv = 0; switch (cmd) { case VIDIOC_QUERYCAP: /* Query capabilities */ { struct v4l2_capability caps; USB_DPRINTF_L4(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "V4L2 ioctl: VIDIOC_QUERYCAP"); bzero(&caps, sizeof (caps)); (void) strncpy((char *)&caps.driver, "usbvc", sizeof (caps.driver)); if (usbvcp->usbvc_reg->dev_product) { (void) strncpy((char *)&caps.card, usbvcp->usbvc_reg->dev_product, sizeof (caps.card)); } else { (void) strncpy((char *)&caps.card, "Generic USB video" "class device", sizeof (caps.card)); } (void) strncpy((char *)&caps.bus_info, "usb", sizeof (caps.bus_info)); caps.version = 1; caps.capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING | V4L2_CAP_READWRITE; USBVC_COPYOUT(caps); break; } case VIDIOC_ENUM_FMT: { struct v4l2_fmtdesc fmtdesc; usbvc_format_group_t *fmtgrp; usbvc_stream_if_t *strm_if; USB_DPRINTF_L4(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "V4L2 ioctl: VIDIOC_ENUM_FMT"); USBVC_COPYIN(fmtdesc); mutex_enter(&usbvcp->usbvc_mutex); strm_if = usbvcp->usbvc_curr_strm; if (fmtdesc.type != V4L2_BUF_TYPE_VIDEO_CAPTURE || fmtdesc.index >= strm_if->fmtgrp_cnt) { rv = EINVAL; mutex_exit(&usbvcp->usbvc_mutex); break; } fmtgrp = &strm_if->format_group[fmtdesc.index]; fmtdesc.pixelformat = fmtgrp->v4l2_pixelformat; USB_DPRINTF_L3(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "V4L2 ioctl: VIDIOC_ENUM_FMT, idx=%d, grpcnt=%d", fmtdesc.index, strm_if->fmtgrp_cnt); switch (fmtgrp->format->bDescriptorSubType) { case VS_FORMAT_MJPEG: fmtdesc.flags = V4L2_FMT_FLAG_COMPRESSED; (void) strncpy(fmtdesc.description, "MJPEG", sizeof (fmtdesc.description)); break; case VS_FORMAT_UNCOMPRESSED: fmtdesc.flags = 0; if (fmtdesc.pixelformat == V4L2_PIX_FMT_YUYV) { (void) strncpy(fmtdesc.description, "YUYV", sizeof (fmtdesc.description)); } else if (fmtdesc.pixelformat == V4L2_PIX_FMT_NV12) { (void) strncpy(fmtdesc.description, "NV12", sizeof (fmtdesc.description)); } else { (void) strncpy(fmtdesc.description, "Unknown format", sizeof (fmtdesc.description)); } break; default: fmtdesc.flags = 0; (void) strncpy(fmtdesc.description, "Unknown format", sizeof (fmtdesc.description)); } mutex_exit(&usbvcp->usbvc_mutex); USBVC_COPYOUT(fmtdesc); break; } case VIDIOC_S_FMT: { struct v4l2_format fmt; usbvc_stream_if_t *strm_if; USB_DPRINTF_L4(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "V4L2 ioctl: VIDIOC_S_FMT"); mutex_enter(&usbvcp->usbvc_mutex); strm_if = usbvcp->usbvc_curr_strm; /* If data I/O is in progress */ if (strm_if->start_polling == 1) { rv = EBUSY; mutex_exit(&usbvcp->usbvc_mutex); break; } mutex_exit(&usbvcp->usbvc_mutex); USBVC_COPYIN(fmt); if (usbvc_v4l2_set_format(usbvcp, &fmt) != USB_SUCCESS) { rv = EFAULT; USB_DPRINTF_L2(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "V4L2 ioctl VIDIOC_S_FMT fail"); } USBVC_COPYOUT(fmt); break; } case VIDIOC_G_FMT: { struct v4l2_format fmt; USB_DPRINTF_L4(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "V4L2 ioctl: VIDIOC_G_FMT"); USBVC_COPYIN(fmt); if ((rv = usbvc_v4l2_get_format(usbvcp, &fmt)) != 0) { break; } USBVC_COPYOUT(fmt); break; } case VIDIOC_REQBUFS: /* for memory mapping IO method */ { struct v4l2_requestbuffers reqbuf; uint_t bufsize; usbvc_stream_if_t *strm_if; USB_DPRINTF_L4(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "V4L2 ioctl: VIDIOC_REQBUFS"); USBVC_COPYIN(reqbuf); if (reqbuf.type != V4L2_BUF_TYPE_VIDEO_CAPTURE || reqbuf.memory != V4L2_MEMORY_MMAP) { rv = EINVAL; break; } mutex_enter(&usbvcp->usbvc_mutex); strm_if = usbvcp->usbvc_curr_strm; if (!strm_if) { mutex_exit(&usbvcp->usbvc_mutex); rv = EINVAL; break; } if (reqbuf.count > USBVC_MAX_MAP_BUF_NUM) { mutex_exit(&usbvcp->usbvc_mutex); USB_DPRINTF_L2(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "V4L2 ioctl: req too many buffers, fail"); rv = EINVAL; break; } /* If some bufs were already allocated */ if (strm_if->buf_map.buf_cnt) { /* * According to v4l2 spec, application can change the * buffer number and also free all buffers if set * count to 0 */ if (reqbuf.count == 0) { if (strm_if->start_polling == 1) { mutex_exit(&usbvcp->usbvc_mutex); usb_pipe_stop_isoc_polling( strm_if->datain_ph, USB_FLAGS_SLEEP); mutex_enter(&usbvcp->usbvc_mutex); strm_if->start_polling = 0; } usbvc_free_map_bufs(usbvcp, strm_if); mutex_exit(&usbvcp->usbvc_mutex); break; } if (reqbuf.count == strm_if->buf_map.buf_cnt) { mutex_exit(&usbvcp->usbvc_mutex); USB_DPRINTF_L2(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "v4l2 ioctls: req the same buffers" " as we already have, just return success"); break; } else { /* * req different number of bufs, according to * v4l2 spec, this is not allowed when there * are some bufs still mapped. */ mutex_exit(&usbvcp->usbvc_mutex); USB_DPRINTF_L2(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "v4l2 ioctls: req different number bufs" "than the exist ones, fail"); rv = EINVAL; break; } } if (reqbuf.count == 0) { mutex_exit(&usbvcp->usbvc_mutex); rv = EINVAL; break; } LE_TO_UINT32(strm_if->ctrl_pc.dwMaxVideoFrameSize, 0, bufsize); if ((reqbuf.count = (uint32_t)usbvc_alloc_map_bufs(usbvcp, strm_if, reqbuf.count, bufsize)) == 0) { mutex_exit(&usbvcp->usbvc_mutex); USB_DPRINTF_L2(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "V4L2 ioctl: VIDIOC_REQBUFS: alloc fail"); rv = EINVAL; break; } mutex_exit(&usbvcp->usbvc_mutex); /* * return buf number that acctually allocated to application */ USBVC_COPYOUT(reqbuf); break; } case VIDIOC_QUERYBUF: /* for memory mapping IO method */ { struct v4l2_buffer buf; usbvc_buf_grp_t *usbvc_bufg; USBVC_COPYIN(buf); USB_DPRINTF_L4(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "V4L2 ioctl: VIDIOC_QUERYBUF: idx=%d", buf.index); mutex_enter(&usbvcp->usbvc_mutex); usbvc_bufg = &usbvcp->usbvc_curr_strm->buf_map; if ((buf.type != V4L2_BUF_TYPE_VIDEO_CAPTURE) || (buf.index >= usbvc_bufg->buf_cnt)) { mutex_exit(&usbvcp->usbvc_mutex); rv = EINVAL; break; } USB_DPRINTF_L3(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "V4L2 ioctl: VIDIOC_QUERYBUF: len=%d", usbvc_bufg->buf_head[buf.index].v4l2_buf.length); usbvc_v4l2_query_buf(usbvcp, &usbvc_bufg->buf_head[buf.index], &buf); mutex_exit(&usbvcp->usbvc_mutex); USBVC_COPYOUT(buf); USB_DPRINTF_L3(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "V4L2 ioctl: VIDIOC_QUERYBUF,(index=%d)len=%d", buf.index, buf.length); break; } case VIDIOC_QBUF: { struct v4l2_buffer buf; usbvc_buf_grp_t *usbvc_bufg; USB_DPRINTF_L4(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "V4L2 ioctl: VIDIOC_QBUF"); USBVC_COPYIN(buf); mutex_enter(&usbvcp->usbvc_mutex); usbvc_bufg = &usbvcp->usbvc_curr_strm->buf_map; if ((buf.type != V4L2_BUF_TYPE_VIDEO_CAPTURE) || (buf.index >= usbvc_bufg->buf_cnt) || (buf.memory != V4L2_MEMORY_MMAP)) { mutex_exit(&usbvcp->usbvc_mutex); USB_DPRINTF_L2(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "V4L2 ioctl: " "VIDIOC_QBUF error:index=%d,type=%d,memory=%d", buf.index, buf.type, buf.memory); rv = EINVAL; break; } rv = usbvc_v4l2_enqueue_buf(usbvcp, &usbvc_bufg->buf_head[buf.index], &buf); if (rv < 0) { mutex_exit(&usbvcp->usbvc_mutex); break; } mutex_exit(&usbvcp->usbvc_mutex); USBVC_COPYOUT(buf); break; } case VIDIOC_DQBUF: { struct v4l2_buffer buf; USB_DPRINTF_L4(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "V4L2 ioctl: VIDIOC_DQBUF"); USBVC_COPYIN(buf); mutex_enter(&usbvcp->usbvc_mutex); if ((rv = usbvc_v4l2_dequeue_buffer(usbvcp, &buf, mode)) != 0) { mutex_exit(&usbvcp->usbvc_mutex); USB_DPRINTF_L2(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "V4L2 ioctl: " "VIDIOC_DQBUF: fail, rv=%d", rv); break; } mutex_exit(&usbvcp->usbvc_mutex); USBVC_COPYOUT(buf); break; } case VIDIOC_STREAMON: { int type; /* v4l2_buf_type */ usbvc_stream_if_t *strm_if; USB_DPRINTF_L4(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "V4L2 ioctl: VIDIOC_STREAMON"); USBVC_COPYIN(type); mutex_enter(&usbvcp->usbvc_mutex); strm_if = usbvcp->usbvc_curr_strm; if (!strm_if) { mutex_exit(&usbvcp->usbvc_mutex); rv = EINVAL; break; } if (type != V4L2_BUF_TYPE_VIDEO_CAPTURE) { mutex_exit(&usbvcp->usbvc_mutex); USB_DPRINTF_L2(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "V4L2 ioctl: " "VIDIOC_STREAMON: fail. Only capture type is" " supported by now."); rv = EINVAL; break; } /* if the first read, open isoc pipe */ if (!strm_if->datain_ph) { if (usbvc_open_isoc_pipe(usbvcp, strm_if) != USB_SUCCESS) { mutex_exit(&usbvcp->usbvc_mutex); USB_DPRINTF_L2(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "V4L2 ioctl:" " first read, open pipe fail"); rv = EINVAL; break; } } /* If it is already started */ if (strm_if->start_polling == 1) { mutex_exit(&usbvcp->usbvc_mutex); break; } /* At present, VIDIOC_STREAMON supports mmap io only. */ if (usbvc_start_isoc_polling(usbvcp, strm_if, V4L2_MEMORY_MMAP) != USB_SUCCESS) { rv = EFAULT; mutex_exit(&usbvcp->usbvc_mutex); break; } strm_if->start_polling = 1; strm_if->stream_on = 1; /* the only place to set this value */ mutex_exit(&usbvcp->usbvc_mutex); break; } case VIDIOC_STREAMOFF: { int type; /* v4l2_buf_type */ usbvc_stream_if_t *strm_if; USB_DPRINTF_L4(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "V4L2 ioctl: VIDIOC_STREAMOFF"); USBVC_COPYIN(type); mutex_enter(&usbvcp->usbvc_mutex); strm_if = usbvcp->usbvc_curr_strm; if (!strm_if) { mutex_exit(&usbvcp->usbvc_mutex); rv = EINVAL; break; } if (type != V4L2_BUF_TYPE_VIDEO_CAPTURE) { mutex_exit(&usbvcp->usbvc_mutex); USB_DPRINTF_L2(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "V4L2 ioctl: " "VIDIOC_STREAMON: fail. Only capture type is " "supported by now."); rv = EINVAL; break; } /* Need close the isoc data pipe if any reads are performed. */ strm_if = usbvcp->usbvc_curr_strm; if (strm_if->start_polling == 1) { mutex_exit(&usbvcp->usbvc_mutex); usb_pipe_stop_isoc_polling(strm_if->datain_ph, USB_FLAGS_SLEEP); mutex_enter(&usbvcp->usbvc_mutex); strm_if->start_polling = 0; } strm_if->stream_on = 0; mutex_exit(&usbvcp->usbvc_mutex); break; } case VIDIOC_ENUMINPUT: { struct v4l2_input input; USB_DPRINTF_L4(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "V4L2 ioctl: ENUMINPUT"); USBVC_COPYIN(input); if (input.index != 0) { /* Support only one INPUT now */ rv = EINVAL; break; } (void) strncpy((char *)input.name, "Camera Terminal", sizeof (input.name)); input.type = V4L2_INPUT_TYPE_CAMERA; USBVC_COPYOUT(input); break; } case VIDIOC_G_INPUT: { int input_idx = 0; /* Support only one input now */ USB_DPRINTF_L4(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "V4L2 ioctl: G_INPUT"); USBVC_COPYOUT(input_idx); break; } case VIDIOC_S_INPUT: { int input_idx; USBVC_COPYIN(input_idx); USB_DPRINTF_L4(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "V4L2 ioctl: S_INPUT"); if (input_idx != 0) { /* Support only one input now */ rv = EINVAL; } break; } /* Query the device that what kinds of video ctrls are supported */ case VIDIOC_QUERYCTRL: { struct v4l2_queryctrl queryctrl; USB_DPRINTF_L4(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "V4L2 ioctl: QUERYCTRL"); USBVC_COPYIN(queryctrl); if (usbvc_v4l2_query_ctrl(usbvcp, &queryctrl) != USB_SUCCESS) { rv = EINVAL; break; } USBVC_COPYOUT(queryctrl); break; } case VIDIOC_G_CTRL: { struct v4l2_control ctrl; USB_DPRINTF_L4(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "V4L2 ioctl: G_CTRL"); USBVC_COPYIN(ctrl); if (usbvc_v4l2_get_ctrl(usbvcp, &ctrl) != USB_SUCCESS) { rv = EINVAL; break; } USBVC_COPYOUT(ctrl); break; } case VIDIOC_S_CTRL: { struct v4l2_control ctrl; USB_DPRINTF_L4(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "V4L2 ioctl: S_CTRL"); USBVC_COPYIN(ctrl); if (usbvc_v4l2_set_ctrl(usbvcp, &ctrl) != USB_SUCCESS) { rv = EINVAL; break; } USBVC_COPYOUT(ctrl); break; } case VIDIOC_S_PARM: { struct v4l2_streamparm parm; usbvc_stream_if_t *strm_if; USB_DPRINTF_L4(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "V4L2 ioctl: VIDIOC_S_PARM"); mutex_enter(&usbvcp->usbvc_mutex); strm_if = usbvcp->usbvc_curr_strm; /* If data I/O is in progress */ if (strm_if->start_polling == 1) { rv = EBUSY; mutex_exit(&usbvcp->usbvc_mutex); break; } mutex_exit(&usbvcp->usbvc_mutex); USBVC_COPYIN(parm); /* Support capture only, so far. */ if (parm.type != V4L2_BUF_TYPE_VIDEO_CAPTURE) { rv = EINVAL; break; } if (usbvc_v4l2_set_parm(usbvcp, &parm) != USB_SUCCESS) { rv = EINVAL; USB_DPRINTF_L2(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "V4L2 ioctl VIDIOC_S_PARM fail"); } USBVC_COPYOUT(parm); break; } case VIDIOC_G_PARM: { struct v4l2_streamparm parm; USB_DPRINTF_L4(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "V4L2 ioctl: VIDIOC_G_PARM"); USBVC_COPYIN(parm); /* Support capture only, so far. */ if (parm.type != V4L2_BUF_TYPE_VIDEO_CAPTURE) { rv = EINVAL; break; } if ((rv = usbvc_v4l2_get_parm(usbvcp, &parm)) != USB_SUCCESS) { break; } USBVC_COPYOUT(parm); break; } /* These ioctls are for analog video standards. */ case VIDIOC_G_STD: case VIDIOC_S_STD: case VIDIOC_ENUMSTD: case VIDIOC_QUERYSTD: rv = EINVAL; USB_DPRINTF_L2(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "usbvc_v4l2_ioctl: not a supported cmd, cmd=%x", cmd); break; default: USB_DPRINTF_L2(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "usbvc_v4l2_ioctl: not a valid cmd value, cmd=%x", cmd); rv = ENOTTY; } USB_DPRINTF_L3(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "usbvc_v4l2_ioctl: exit, rv=%d", rv); return (rv); } /* * Convert GUID in uncompressed format descriptor to the pixelformat element * in struct v4l2_pix_format */ uint32_t usbvc_v4l2_guid2fcc(uint8_t *guid) { uint32_t ret; uint8_t y[16] = USBVC_FORMAT_GUID_YUY2; uint8_t n[16] = USBVC_FORMAT_GUID_NV12; if (!memcmp((void *)guid, (void *) &y[0], 16)) { ret = V4L2_PIX_FMT_YUYV; return (ret); } if (!memcmp((void *)guid, (void *) &n, 16)) { ret = V4L2_PIX_FMT_NV12; return (ret); } return (0); } /* * Find a frame which has the closest image size as the input args * (width, height) */ static usbvc_frames_t * usbvc_match_image_size(uint32_t width, uint32_t height, usbvc_format_group_t *fmtgrp) { uint32_t w, h, diff, sz, i; usbvc_frames_t *frame = NULL; usbvc_frame_descr_t *descr; diff = 0xffffffff; for (i = 0; i < fmtgrp->frame_cnt; i++) { descr = fmtgrp->frames[i].descr; if (descr == NULL) { continue; } LE_TO_UINT16(descr->wWidth, 0, w); LE_TO_UINT16(descr->wHeight, 0, h); sz = min(w, width) * min(h, height); sz = (w * h + width * height - sz * 2); if (sz < diff) { frame = &fmtgrp->frames[i]; diff = sz; } if (diff == 0) { return (frame); } } return (frame); } /* Implement ioctl VIDIOC_S_FMT, set a video format */ static int usbvc_v4l2_set_format(usbvc_state_t *usbvcp, struct v4l2_format *format) { usbvc_vs_probe_commit_t ctrl, ctrl_max, ctrl_min, ctrl_curr; usbvc_stream_if_t *strm_if; usbvc_format_group_t *fmtgrp; usbvc_frames_t *frame; uint32_t w, h, interval, bandwidth; uint8_t type, i; mutex_enter(&usbvcp->usbvc_mutex); /* * Get the first stream interface. Todo: deal with multi stream * interfaces. */ strm_if = usbvcp->usbvc_curr_strm; USB_DPRINTF_L4(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "usbvc_v4l2_set_format: strm_if->fmtgrp_cnt=%d", strm_if->fmtgrp_cnt); /* Find the proper format group according to compress type and guid */ for (i = 0; i < strm_if->fmtgrp_cnt; i++) { fmtgrp = &strm_if->format_group[i]; /* * If v4l2_pixelformat is NULL, then that means there is not * a parsed format in format_group[i]. */ if (!fmtgrp->v4l2_pixelformat || fmtgrp->frame_cnt == 0) { USB_DPRINTF_L3(PRINT_MASK_DEVCTRL, usbvcp->usbvc_log_handle, "usbvc_set_default_stream_fmt: no frame, fail"); continue; } type = fmtgrp->format->bDescriptorSubType; USB_DPRINTF_L3(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "usbvc_v4l2_set_format: type =%x, i =%d", type, i); if ((type == VS_FORMAT_MJPEG) || (type == VS_FORMAT_UNCOMPRESSED)) { if (format->fmt.pix.pixelformat == fmtgrp->v4l2_pixelformat) { break; } } } if (i >= strm_if->fmtgrp_cnt) { mutex_exit(&usbvcp->usbvc_mutex); USB_DPRINTF_L2(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "usbvc_v4l2_set_format: can't find a proper format, " "pixelformat=%x", format->fmt.pix.pixelformat); return (USB_FAILURE); } fmtgrp = &strm_if->format_group[i]; frame = usbvc_match_image_size(format->fmt.pix.width, format->fmt.pix.height, fmtgrp); if (frame == NULL) { mutex_exit(&usbvcp->usbvc_mutex); USB_DPRINTF_L2(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "usbvc_v4l2_set_format: can't find a proper frame, rw=%d, " "rh=%d", format->fmt.pix.width, format->fmt.pix.height); return (USB_FAILURE); } /* frame interval */ LE_TO_UINT32(frame->descr->dwDefaultFrameInterval, 0, interval); USB_DPRINTF_L3(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "usbvc_v4l2_set_format: Default Frame Interval=%x", interval); /* * Begin negotiate formats. */ bzero((void *)&ctrl, sizeof (usbvc_vs_probe_commit_t)); /* dwFrameInterval is fixed */ ctrl.bmHint[0] = 1; ctrl.bFormatIndex = fmtgrp->format->bFormatIndex; ctrl.bFrameIndex = frame->descr->bFrameIndex; UINT32_TO_LE(interval, 0, ctrl.dwFrameInterval); mutex_exit(&usbvcp->usbvc_mutex); /* Probe, just a test before the real try */ if (usbvc_vs_set_probe_commit(usbvcp, strm_if, &ctrl, VS_PROBE_CONTROL) != USB_SUCCESS) { USB_DPRINTF_L2(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "usbvc_v4l2_set_format: set probe failed"); return (USB_FAILURE); } /* Get max values */ if (usbvc_vs_get_probe(usbvcp, strm_if, &ctrl_max, GET_MAX) != USB_SUCCESS) { USB_DPRINTF_L2(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "usbvc_v4l2_set_format: get probe MAX failed"); return (USB_FAILURE); } /* Use the best quality first */ bcopy(&ctrl_max.wCompQuality, &ctrl.wCompQuality, 2); /* * By now, we've get some parametres of ctrl req, next try to set ctrl. */ for (i = 0; i < 2; i++) { /* Probe */ if (usbvc_vs_set_probe_commit(usbvcp, strm_if, &ctrl, VS_PROBE_CONTROL) != USB_SUCCESS) { return (USB_FAILURE); } /* Get current value after probe */ if (usbvc_vs_get_probe(usbvcp, strm_if, &ctrl_curr, GET_CUR) != USB_SUCCESS) { return (USB_FAILURE); } LE_TO_UINT32(ctrl_curr.dwMaxPayloadTransferSize, 0, bandwidth); USB_DPRINTF_L3(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "usbvc_v4l2_set_format: bandwidth=%x", bandwidth); /* * If the bandwidth does not exceed the max value of all the * alternatives in this interface, we done. */ if (bandwidth <= strm_if->max_isoc_payload) { break; } if (i >= 1) { return (USB_FAILURE); } /* Get minimum values since the bandwidth is not enough */ if (usbvc_vs_get_probe(usbvcp, strm_if, &ctrl_min, GET_MIN) != USB_SUCCESS) { USB_DPRINTF_L2(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "usbvc_v4l2_set_format: get probe MIN failed"); return (USB_FAILURE); } /* To keep simple, just use some minimum values to try again */ bcopy(&ctrl_min.wKeyFrameRate, &ctrl_curr.wKeyFrameRate, 2); bcopy(&ctrl_min.wPFrameRate, &ctrl_curr.wPFrameRate, 2); bcopy(&ctrl_min.wCompWindowSize, &ctrl_curr.wCompWindowSize, 2); bcopy(&ctrl_max.wCompQuality, &ctrl_curr.wCompQuality, 2); bcopy(&ctrl_curr, &ctrl, sizeof (usbvc_vs_probe_commit_t)); } bcopy(&ctrl_curr, &ctrl, sizeof (usbvc_vs_probe_commit_t)); /* commit the values we negotiated above */ if (usbvc_vs_set_probe_commit(usbvcp, strm_if, &ctrl, VS_COMMIT_CONTROL) != USB_SUCCESS) { USB_DPRINTF_L2(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "usbvc_v4l2_set_format: set probe failed, i=%d", i); return (USB_FAILURE); } mutex_enter(&usbvcp->usbvc_mutex); /* * It's good to check index here before use it. bFormatIndex is based * on 1, and format_group[i] is based on 0, so minus 1 */ i = ctrl.bFormatIndex - 1; if (i < strm_if->fmtgrp_cnt) { strm_if->cur_format_group = &strm_if->format_group[i]; } else { USB_DPRINTF_L2(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "usbvc_v4l2_set_format: format index out of range"); mutex_exit(&usbvcp->usbvc_mutex); return (USB_FAILURE); } /* bFrameIndex is based on 1, and frames[i] is based on 0, so minus 1 */ i = ctrl.bFrameIndex -1; if (i < strm_if->cur_format_group->frame_cnt) { strm_if->cur_format_group->cur_frame = &strm_if->cur_format_group->frames[i]; } else { mutex_exit(&usbvcp->usbvc_mutex); USB_DPRINTF_L2(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "usbvc_v4l2_set_format: frame index out of range"); return (USB_FAILURE); } /* * by now, the video format is set successfully. record the current * setting to strm_if->ctrl_pc */ bcopy(&ctrl_curr, &strm_if->ctrl_pc, sizeof (usbvc_vs_probe_commit_t)); format->fmt.pix.colorspace = fmtgrp->v4l2_color; format->fmt.pix.field = V4L2_FIELD_NONE; format->fmt.pix.priv = 0; LE_TO_UINT16(frame->descr->wWidth, 0, w); LE_TO_UINT16(frame->descr->wHeight, 0, h); format->fmt.pix.width = w; format->fmt.pix.height = h; format->fmt.pix.bytesperline = fmtgrp->v4l2_bpp * w; LE_TO_UINT32(strm_if->ctrl_pc.dwMaxVideoFrameSize, 0, format->fmt.pix.sizeimage); mutex_exit(&usbvcp->usbvc_mutex); USB_DPRINTF_L4(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "usbvc_v4l2_set_format: dwMaxVideoFrameSize=%x, w=%x, h=%x", format->fmt.pix.sizeimage, w, h); return (USB_SUCCESS); } /* Implement ioctl VIDIOC_G_FMT, get the current video format */ static int usbvc_v4l2_get_format(usbvc_state_t *usbvcp, struct v4l2_format *format) { usbvc_stream_if_t *strm_if; usbvc_format_group_t *fmtgrp; uint16_t w, h; if (format->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) { return (EINVAL); } mutex_enter(&usbvcp->usbvc_mutex); /* get the current interface. */ strm_if = usbvcp->usbvc_curr_strm; fmtgrp = strm_if->cur_format_group; if (!fmtgrp || !fmtgrp->cur_frame) { mutex_exit(&usbvcp->usbvc_mutex); USB_DPRINTF_L2(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "usbvc_v4l2_get_format: fail, no current format or frame," "fmtgrp=%p", (void *)fmtgrp); return (EINVAL); } format->fmt.pix.colorspace = fmtgrp->v4l2_color; format->fmt.pix.priv = 0; format->fmt.pix.pixelformat = fmtgrp->v4l2_pixelformat; LE_TO_UINT16(fmtgrp->cur_frame->descr->wWidth, 0, w); LE_TO_UINT16(fmtgrp->cur_frame->descr->wHeight, 0, h); USB_DPRINTF_L3(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "v4l2 ioctl get format "); format->fmt.pix.width = w; format->fmt.pix.height = h; format->fmt.pix.field = V4L2_FIELD_NONE; format->fmt.pix.bytesperline = fmtgrp->v4l2_bpp * w; LE_TO_UINT32(strm_if->ctrl_pc.dwMaxVideoFrameSize, 0, format->fmt.pix.sizeimage); mutex_exit(&usbvcp->usbvc_mutex); return (0); } /* * Convert color space descriptor's bColorPrimaries to the colorspace element * in struct v4l2_pix_format */ uint8_t usbvc_v4l2_colorspace(uint8_t color_prim) { if (color_prim < NELEM(color_primaries)) { return (color_primaries[color_prim]); } return (0); } /* Implement ioctl VIDIOC_QUERYBUF, get the buf status */ static void usbvc_v4l2_query_buf(usbvc_state_t *usbvcp, usbvc_buf_t *usbvc_buf, struct v4l2_buffer *v4l2_buf) { ASSERT(mutex_owned(&usbvcp->usbvc_mutex)); bcopy(&(usbvc_buf->v4l2_buf), v4l2_buf, sizeof (struct v4l2_buffer)); USB_DPRINTF_L4(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "usbvc_v4l2_query_buf: uv_buf_len=%d, len=%d", usbvc_buf->v4l2_buf.length, v4l2_buf->length); if (usbvc_buf->status >= USBVC_BUF_MAPPED) { v4l2_buf->flags |= V4L2_BUF_FLAG_MAPPED; } switch (usbvc_buf->status) { case USBVC_BUF_DONE: case USBVC_BUF_ERR: v4l2_buf->flags |= V4L2_BUF_FLAG_DONE; break; case USBVC_BUF_EMPTY: v4l2_buf->flags |= V4L2_BUF_FLAG_QUEUED; break; case USBVC_BUF_INIT: default: break; } } /* Implement ioctl VIDIOC_QBUF, queue a empty buf to the free list */ static int usbvc_v4l2_enqueue_buf(usbvc_state_t *usbvcp, usbvc_buf_t *usbvc_buf, struct v4l2_buffer *buf) { usbvc_buf_t *donebuf; boolean_t queued = B_FALSE; usbvc_buf_grp_t *bufgrp; ASSERT(mutex_owned(&usbvcp->usbvc_mutex)); bufgrp = &usbvcp->usbvc_curr_strm->buf_map; if (usbvc_buf == bufgrp->buf_filling) { USB_DPRINTF_L3(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "enqueue_buffer(%d) , want to queue buf_filling, " "just return success", buf->index); return (0); } if (!list_is_empty(&bufgrp->uv_buf_done)) { donebuf = (usbvc_buf_t *)list_head(&bufgrp->uv_buf_done); while (donebuf) { if (donebuf == &(bufgrp->buf_head[buf->index])) { queued = B_TRUE; break; } donebuf = (usbvc_buf_t *)list_next(&bufgrp->uv_buf_done, donebuf); } } if (queued) { USB_DPRINTF_L3(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "enqueue_buffer(%d), still in done list, don't insert to" " free list", buf->index); return (0); } if (usbvc_buf->status == USBVC_BUF_EMPTY) { USB_DPRINTF_L3(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "enqueue buffer(%d), already queued.", buf->index); return (0); } if (usbvc_buf->status < USBVC_BUF_MAPPED) { USB_DPRINTF_L2(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "enqueue buffer(%d), state error, not mapped.", buf->index); return (EINVAL); } /* * The buf is put to the buf free list when allocated, so, if the buf * is the first time to enqueue, just change the state to empty is * enough. */ if (usbvc_buf->status == USBVC_BUF_MAPPED) { USB_DPRINTF_L3(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "queue_buffer(%d), 1st time queue this buf", buf->index); usbvc_buf->status = USBVC_BUF_EMPTY; } else { USB_DPRINTF_L3(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "enqueue_buffer(%d) , USBVC_BUF_EMPTY", buf->index); usbvc_buf->status = USBVC_BUF_EMPTY; usbvc_buf->v4l2_buf.bytesused = 0; list_insert_tail(&bufgrp->uv_buf_free, usbvc_buf); } buf->flags &= ~V4L2_BUF_FLAG_DONE; buf->flags |= V4L2_BUF_FLAG_MAPPED | V4L2_BUF_FLAG_QUEUED; return (0); } /* Implement ioctl VIDIOC_DQBUF, pick a buf from done list */ static int usbvc_v4l2_dequeue_buffer(usbvc_state_t *usbvcp, struct v4l2_buffer *buf, int mode) { usbvc_buf_t *buf_done; ASSERT(mutex_owned(&usbvcp->usbvc_mutex)); USB_DPRINTF_L4(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "usbvc_v4l2_dequeue_buffer: idx=%x", buf->index); /* v4l2 spec: app just set type and memory field */ if ((buf->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) || (buf->memory != V4L2_MEMORY_MMAP)) { return (EINVAL); } if ((mode & (O_NDELAY|O_NONBLOCK)) && (list_is_empty(&usbvcp->usbvc_curr_strm->buf_map.uv_buf_done))) { /* non-blocking */ return (EAGAIN); } /* no available buffers, block here */ while (list_is_empty(&usbvcp->usbvc_curr_strm->buf_map.uv_buf_done)) { USB_DPRINTF_L3(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "usbvc_v4l2_dequeue_buffer: wait for done buf"); if (cv_wait_sig(&usbvcp->usbvc_mapio_cv, &usbvcp->usbvc_mutex) <= 0) { /* no done buf and is signaled */ return (EINTR); } if (usbvcp->usbvc_dev_state != USB_DEV_ONLINE) { /* Device is disconnected. */ return (EINTR); } } buf_done = list_head(&usbvcp->usbvc_curr_strm->buf_map.uv_buf_done); list_remove(&usbvcp->usbvc_curr_strm->buf_map.uv_buf_done, buf_done); /* * just copy the v4l2_buf structure because app need only the index * value to locate the mapped memory */ bcopy(&buf_done->v4l2_buf, buf, sizeof (struct v4l2_buffer)); buf->flags |= V4L2_BUF_FLAG_DONE | V4L2_BUF_FLAG_MAPPED; buf->bytesused = buf_done->filled; USB_DPRINTF_L4(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "usbvc_v4l2_dequeue_buffer: bytesused=%d, idx=%x, status=%d", buf->bytesused, buf->index, buf_done->status); return (0); } /* * Check if a ctrl_id is supported by the device, if yes, find the * corresponding processing unit and fill usbvc_v4l2_ctrl_t */ static int usbvc_v4l2_match_ctrl(usbvc_state_t *usbvcp, usbvc_v4l2_ctrl_t *ctrl, uint32_t ctrl_id) { uint8_t idx; usbvc_units_t *unit; uchar_t bit; USB_DPRINTF_L3(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "usbvc_v4l2_match_ctrl: ctrl_id=%x", ctrl_id); if (ctrl_id >= V4L2_CID_PRIVATE_BASE) { return (USB_FAILURE); } if (ctrl_id < V4L2_CID_BASE) { return (USB_FAILURE); } /* get the idx of ctrl array usbvc_v4l2_ctrl */ idx = ctrl_id - V4L2_CID_BASE; if (ctrl_id == V4L2_CID_GAMMA) { /* The 4th one is for Gamma ctrl */ bit = usbvc_v4l2_ctrls[4].bit; } else if ((ctrl_id >= V4L2_CID_BRIGHTNESS) && (ctrl_id <= V4L2_CID_HUE)) { /* The idxth one is for this ctrl */ bit = usbvc_v4l2_ctrls[idx].bit; } else { return (USB_FAILURE); } unit = (usbvc_units_t *)list_head(&usbvcp->usbvc_unit_list); /* * Check if there is a processing unit supportting this ctrl. * Todo: check if the ctrl and the unit is really for the right * stream interface in case of multi stream interfaces. */ while (unit != NULL) { if (unit->descr->bDescriptorSubType == VC_PROCESSING_UNIT) { if (bit >= (unit->descr->unit.processing.bControlSize * 8)) { /* * If this unit's bmControls size is smaller * than bit, then next */ unit = (usbvc_units_t *) list_next(&usbvcp->usbvc_unit_list, unit); continue; } else { /* * The first two bytes of bmControls are * for ctrls */ if ((bit < 8) && unit->bmControls[0] & (0x1 << bit)) { break; } if ((bit >= 8 && bit < 16) && unit->bmControls[1] & (0x1 << bit)) { break; } } } unit = (usbvc_units_t *)list_next(&usbvcp->usbvc_unit_list, unit); } if (unit == NULL) { return (USB_FAILURE); } ctrl->entity_id = unit->descr->bUnitID; if (ctrl_id == V4L2_CID_GAMMA) { ctrl->ctrl_map = &usbvc_v4l2_ctrls[4]; } else { ctrl->ctrl_map = &usbvc_v4l2_ctrls[idx]; } return (USB_SUCCESS); } /* * Implement ioctl VIDIOC_QUERYCTRL, query the ctrl types that the device * supports */ static int usbvc_v4l2_query_ctrl(usbvc_state_t *usbvcp, struct v4l2_queryctrl *queryctrl) { usbvc_v4l2_ctrl_t ctrl; mblk_t *data; char req[16]; if (usbvc_v4l2_match_ctrl(usbvcp, &ctrl, queryctrl->id) != USB_SUCCESS) { return (USB_FAILURE); } if ((data = allocb(ctrl.ctrl_map->len, BPRI_LO)) == NULL) { return (USB_FAILURE); } if (usbvc_vc_get_ctrl(usbvcp, GET_MIN, ctrl.entity_id, ctrl.ctrl_map->selector, ctrl.ctrl_map->len, data) != USB_SUCCESS) { (void) strncpy(&req[0], "GET_MIN", sizeof (req)); goto fail; } LE_TO_UINT16(data->b_rptr, 0, queryctrl->minimum); if (usbvc_vc_get_ctrl(usbvcp, GET_MAX, ctrl.entity_id, ctrl.ctrl_map->selector, ctrl.ctrl_map->len, data) != USB_SUCCESS) { (void) strncpy(&req[0], "GET_MAX", sizeof (req)); goto fail; } LE_TO_UINT16(data->b_rptr, 0, queryctrl->maximum); if (usbvc_vc_get_ctrl(usbvcp, GET_RES, ctrl.entity_id, ctrl.ctrl_map->selector, ctrl.ctrl_map->len, data) != USB_SUCCESS) { (void) strncpy(&req[0], "GET_RES", sizeof (req)); goto fail; } LE_TO_UINT16(data->b_rptr, 0, queryctrl->step); if (usbvc_vc_get_ctrl(usbvcp, GET_DEF, ctrl.entity_id, ctrl.ctrl_map->selector, ctrl.ctrl_map->len, data) != USB_SUCCESS) { (void) strncpy(&req[0], "GET_DEF", sizeof (req)); goto fail; } LE_TO_UINT16(data->b_rptr, 0, queryctrl->default_value); (void) strncpy(queryctrl->name, ctrl.ctrl_map->name, sizeof (queryctrl->name)); queryctrl->type = ctrl.ctrl_map->type; queryctrl->flags = 0; if (data) { freemsg(data); } return (USB_SUCCESS); fail: if (data) { freemsg(data); } USB_DPRINTF_L2(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "usbvc_v4l2_query_ctrl: fail when %s", req); return (USB_FAILURE); } /* Implement ioctl VIDIOC_G_CTRL, get current ctrl */ static int usbvc_v4l2_get_ctrl(usbvc_state_t *usbvcp, struct v4l2_control *v4l2_ctrl) { usbvc_v4l2_ctrl_t ctrl; mblk_t *data; if (usbvc_v4l2_match_ctrl(usbvcp, &ctrl, v4l2_ctrl->id) != USB_SUCCESS) { return (USB_FAILURE); } if ((data = allocb(ctrl.ctrl_map->len, BPRI_LO)) == NULL) { return (USB_FAILURE); } if (usbvc_vc_get_ctrl(usbvcp, GET_CUR, ctrl.entity_id, ctrl.ctrl_map->selector, ctrl.ctrl_map->len, data) != USB_SUCCESS) { if (data) { freemsg(data); } USB_DPRINTF_L2(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "usbvc_v4l2_get_ctrl: fail"); return (USB_FAILURE); } LE_TO_UINT16(data->b_rptr, 0, v4l2_ctrl->value); if (data) { freemsg(data); } return (USB_SUCCESS); } /* Implement ioctl VIDIOC_S_CTRL */ static int usbvc_v4l2_set_ctrl(usbvc_state_t *usbvcp, struct v4l2_control *v4l2_ctrl) { usbvc_v4l2_ctrl_t ctrl; mblk_t *data; if (usbvc_v4l2_match_ctrl(usbvcp, &ctrl, v4l2_ctrl->id) != USB_SUCCESS) { return (USB_FAILURE); } if ((data = allocb(ctrl.ctrl_map->len, BPRI_LO)) == NULL) { return (USB_FAILURE); } UINT16_TO_LE(v4l2_ctrl->value, 0, data->b_wptr); data->b_wptr += 2; if (usbvc_vc_set_ctrl(usbvcp, SET_CUR, ctrl.entity_id, ctrl.ctrl_map->selector, ctrl.ctrl_map->len, data) != USB_SUCCESS) { if (data) { freemsg(data); } USB_DPRINTF_L2(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "usbvc_v4l2_set_ctrl: fail"); return (USB_FAILURE); } if (data) { freemsg(data); } return (USB_SUCCESS); } /* For the given interval, find the closest frame interval to it. */ static uint32_t usbvc_find_interval(usbvc_frames_t *frame, uint32_t interval) { uint32_t step, i, closest, index, approx1, approx2; /* * for continuous case, there is a min and a max, and also a step * value. The available intervals are those between min and max * values. */ if (!frame->descr->bFrameIntervalType) { step = frame->dwFrameIntervalStep; if (step == 0) { /* a malfunction device */ return (0); } else if (interval <= frame->dwMinFrameInterval) { /* return the most possible interval we can handle */ return (frame->dwMinFrameInterval); } else if (interval >= frame->dwMaxFrameInterval) { /* return the most possible interval we can handle */ return (frame->dwMaxFrameInterval); } approx1 = (interval / step) * step; approx2 = approx1 + step; closest = ((interval - approx1) < (approx2 - interval)) ? approx1 : approx2; return (closest); } /* * for discrete case, search all the available intervals, find the * closest one. */ closest = 0; approx2 = (uint32_t)-1; for (index = 0; index < frame->descr->bFrameIntervalType; index++) { LE_TO_UINT32(frame->dwFrameInterval, index * 4, i); approx1 = (i > interval) ? (i - interval) : (interval - i); if (approx1 == 0) { /* find the matched one, return it immediately */ return (i); } if (approx1 < approx2) { approx2 = approx1; closest = i; } } return (closest); } /* Implement ioctl VIDIOC_S_PARM. Support capture only, so far. */ static int usbvc_v4l2_set_parm(usbvc_state_t *usbvcp, struct v4l2_streamparm *parm) { usbvc_stream_if_t *strm_if; usbvc_format_group_t *cur_fmt; usbvc_frames_t *cur_frame; uint32_t n, d, c, i; usbvc_vs_probe_commit_t ctrl; mutex_enter(&usbvcp->usbvc_mutex); strm_if = usbvcp->usbvc_curr_strm; if (!strm_if->cur_format_group || !strm_if->cur_format_group->cur_frame) { USB_DPRINTF_L2(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "usbvc_v4l2_set_parm: current format or" " frame is not set. cur_fmt=%p", (void *)strm_if->cur_format_group); mutex_exit(&usbvcp->usbvc_mutex); return (USB_FAILURE); } cur_fmt = strm_if->cur_format_group; cur_frame = cur_fmt->cur_frame; mutex_exit(&usbvcp->usbvc_mutex); if (parm->parm.capture.readbuffers > USBVC_MAX_READ_BUF_NUM) { USB_DPRINTF_L2(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "usbvc_v4l2_set_parm: ask too many read buffers," " readbuffers=%d", parm->parm.capture.readbuffers); return (USB_FAILURE); } n = parm->parm.capture.timeperframe.numerator; d = parm->parm.capture.timeperframe.denominator; /* check the values passed in, in case of zero devide */ if (d == 0) { USB_DPRINTF_L2(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "usbvc_v4l2_set_parm: invalid denominator=%d", d); return (USB_FAILURE); } /* * UVC frame intervals are in 100ns units, need convert from * 1s unit to 100ns unit */ c = USBVC_FRAME_INTERVAL_DENOMINATOR; /* check the values passed in, in case of overflow */ if (n / d >= ((uint32_t)-1) / c) { USB_DPRINTF_L2(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "usbvc_v4l2_set_parm: overflow, numerator=%d," " denominator=%d", n, d); return (USB_FAILURE); } USB_DPRINTF_L3(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "usbvc_v4l2_set_parm: numerator=%d, denominator=%d", n, d); /* compute the interval in 100ns unit */ if (n <= ((uint32_t)-1) / c) { i = (n * c) / d; } else { do { n >>= 1; d >>= 1; /* decrease both n and d, in case overflow */ } while (n && d && n > ((uint32_t)-1) / c); if (!d) { USB_DPRINTF_L2(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "usbvc_v4l2_set_parm: can't compute interval," " denominator=%d", d); return (USB_FAILURE); } i = (n * c) / d; } USB_DPRINTF_L3(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "usbvc_v4l2_set_parm: want interval=%d, n=%d, d=%d, c=%d", i, n, d, c); /* * Begin negotiate frame intervals. */ bcopy(&strm_if->ctrl_pc, &ctrl, sizeof (usbvc_vs_probe_commit_t)); i = usbvc_find_interval(cur_frame, i); if (i == 0) { USB_DPRINTF_L2(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "usbvc_v4l2_set_parm: can not find an proper interval." " i=%d, n=%d, d=%d", i, n, d); return (USB_FAILURE); } USB_DPRINTF_L3(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "usbvc_v4l2_set_parm: get interval=%d", i); UINT32_TO_LE(i, 0, ctrl.dwFrameInterval); /* Probe, just a test before the real try */ if (usbvc_vs_set_probe_commit(usbvcp, strm_if, &ctrl, VS_PROBE_CONTROL) != USB_SUCCESS) { USB_DPRINTF_L2(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "usbvc_v4l2_set_parm: set probe failed"); return (USB_FAILURE); } /* Commit the frame interval. */ if (usbvc_vs_set_probe_commit(usbvcp, strm_if, &ctrl, VS_COMMIT_CONTROL) != USB_SUCCESS) { USB_DPRINTF_L2(PRINT_MASK_IOCTL, usbvcp->usbvc_log_handle, "usbvc_v4l2_set_parm: set commit failed"); return (USB_FAILURE); } bcopy(&ctrl, &strm_if->ctrl_pc, sizeof (usbvc_vs_probe_commit_t)); LE_TO_UINT32(ctrl.dwFrameInterval, 0, i); parm->parm.capture.timeperframe.numerator = i; parm->parm.capture.timeperframe.denominator = c; mutex_enter(&usbvcp->usbvc_mutex); /* * According to ioctl VIDIOC_S_PARM, zero value of readbuffers will not * be set. And the current value is expected to return to application. */ if (parm->parm.capture.readbuffers != 0) { strm_if->buf_read_num = parm->parm.capture.readbuffers; } else { parm->parm.capture.readbuffers = strm_if->buf_read_num; } mutex_exit(&usbvcp->usbvc_mutex); return (USB_SUCCESS); } /* Implement ioctl VIDIOC_G_PARM. */ static int usbvc_v4l2_get_parm(usbvc_state_t *usbvcp, struct v4l2_streamparm *parm) { usbvc_stream_if_t *strm_if; uint32_t n, d; bzero(parm, sizeof (*parm)); mutex_enter(&usbvcp->usbvc_mutex); strm_if = usbvcp->usbvc_curr_strm; /* return the actual number of buffers allocated for read() I/O */ parm->parm.capture.readbuffers = strm_if->buf_read.buf_cnt; /* in 100ns units */ LE_TO_UINT32(strm_if->ctrl_pc.dwFrameInterval, 0, n); mutex_exit(&usbvcp->usbvc_mutex); /* * According to UVC payload specs, the dwFrameInterval in frame * descriptors is in 100ns unit. */ d = USBVC_FRAME_INTERVAL_DENOMINATOR; parm->parm.capture.timeperframe.numerator = n; parm->parm.capture.timeperframe.denominator = d; /* Support capture only, so far. */ parm->type = V4L2_BUF_TYPE_VIDEO_CAPTURE; parm->parm.capture.capability = V4L2_CAP_TIMEPERFRAME; parm->parm.capture.capturemode = 0; /* no high quality imaging mode */ parm->parm.capture.extendedmode = 0; /* no driver specific parameters */ /* Always success for current support of this command */ return (USB_SUCCESS); }