static int
dr_check_io_refs(dr_handle_t *hp, dr_common_unit_t **devlist, int devnum)
{
register int i, reftotal = 0;
static fn_t f = "dr_check_io_refs";
for (i = 0; i < devnum; i++) {
dr_io_unit_t *ip = (dr_io_unit_t *)devlist[i];
dev_info_t *dip;
int ref;
sbd_error_t *err;
err = drmach_get_dip(ip->sbi_cm.sbdev_id, &dip);
if (err)
DRERR_SET_C(&ip->sbi_cm.sbdev_error, &err);
else if (dip != NULL) {
ref = 0;
ASSERT(e_ddi_branch_held(dip));
dr_check_devices(dip, &ref, hp, NULL, NULL, 0);
hp->h_err = NULL;
if (ref) {
dr_dev_err(CE_WARN, &ip->sbi_cm, ESBD_BUSY);
}
PR_IO("%s: dip(%s) ref = %d\n",
f, ddi_get_name(dip), ref);
reftotal += ref;
} else {
PR_IO("%s: NO dip for id (0x%x)\n",
f, (uint_t)(uintptr_t)ip->sbi_cm.sbdev_id);
}
}
return (reftotal);
}
void start_capturing(fimc_buf_t * bufs)
{
unsigned int i;
enum v4l2_buf_type type;
struct v4l2_plane planes[VIDEO_MAX_PLANES];
PR_IO(VIDIOC_QBUF);
for (i = 0; i < n_buffers; ++i) {
struct v4l2_buffer buf;
CLEAR(buf);
buf.type = CAPTURE_BUFFER_TYPE;
buf.memory = V4L2_MEMORY_MMAP;
buf.index = i;
if (buf.type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
buf.m.planes = planes;
buf.length = 1;
buf.m.planes[0].bytesused = bufs[i].size[0];
}
if (-1 == xioctl(g_file_desc, VIDIOC_QBUF, &buf))
errno_exit("VIDIOC_QBUF");
}
PR_IO(VIDIOC_STREAMON);
type = CAPTURE_BUFFER_TYPE;
if (-1 == xioctl(g_file_desc, VIDIOC_STREAMON, &type))
errno_exit("VIDIOC_STREAMON");
}
static int read_frame(void)
{
struct v4l2_buffer buf;
struct v4l2_plane planes[VIDEO_MAX_PLANES];
int index;
CLEAR(buf);
buf.type = CAPTURE_BUFFER_TYPE;
buf.memory = V4L2_MEMORY_MMAP;
PR_IO(VIDIOC_DQBUF);
if (-1 == xioctl(g_file_desc, VIDIOC_DQBUF, &buf)) {
switch (errno) {
case EAGAIN:
return 0;
case EIO:
/* Could ignore EIO, see spec. */
/* fall through */
default:
errno_exit("VIDIOC_DQBUF");
}
}
assert(buf.index < n_buffers);
process_image(buffers[buf.index].addr[0]);
index = buf.index;
PR_IO(VIDIOC_QBUF);
CLEAR(buf);
buf.type = CAPTURE_BUFFER_TYPE;
buf.memory = V4L2_MEMORY_MMAP;
buf.index = index;
if (buf.type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
buf.m.planes = planes;
buf.length = 1;
buf.m.planes[0].bytesused = buffers[index].size[0];
}
if (-1 == xioctl(g_file_desc, VIDIOC_QBUF, &buf))
errno_exit("VIDIOC_QBUF");
return 1;
}
int
dr_pre_release_io(dr_handle_t *hp,
dr_common_unit_t **devlist, int devnum)
{
static fn_t f = "dr_pre_release_io";
int d;
ASSERT(devnum > 0);
/* fail if any I/O device pre-release fails */
for (d = 0; d < devnum; d++) {
dr_io_unit_t *ip = (dr_io_unit_t *)devlist[d];
if ((hp->h_err = drmach_io_pre_release(
ip->sbi_cm.sbdev_id)) != 0) {
return (-1);
}
}
for (d = 0; d < devnum; d++) {
dr_io_unit_t *ip = (dr_io_unit_t *)devlist[d];
sbd_error_t *err;
err = drmach_release(ip->sbi_cm.sbdev_id);
if (err) {
DRERR_SET_C(&ip->sbi_cm.sbdev_error,
&err);
return (-1);
}
}
/* fail if any I/O devices are still referenced */
if (dr_check_io_refs(hp, devlist, devnum) > 0) {
PR_IO("%s: failed - I/O devices ref'd\n", f);
/* recover before return error */
for (d = 0; d < devnum; d++) {
dr_io_unit_t *ip = (dr_io_unit_t *)devlist[d];
sbd_error_t *err;
err = drmach_io_unrelease(ip->sbi_cm.sbdev_id);
if (err) {
DRERR_SET_C(&ip->sbi_cm.sbdev_error, &err);
return (-1);
}
}
return (-1);
}
return (0);
}
/*ARGSUSED*/
int
dr_post_detach_io(dr_handle_t *hp, dr_common_unit_t **devlist, int devnum)
{
register int i;
int rv = 0;
static fn_t f = "dr_post_detach_io";
ASSERT(devnum > 0);
for (i = 0; i < devnum; i++) {
dr_common_unit_t *cp = devlist[i];
if (cp->sbdev_error != NULL) {
PR_IO("%s: Failed\n", f);
rv = -1;
break;
}
}
return (rv);
}
static void start_control_testing(void)
{
struct v4l2_format fmt;
int i, ret = 0;
PR_IO(VIDIOC_G_FMT);
fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
ret = ioctl(g_file_desc, VIDIOC_G_FMT, &fmt);
if (ret) {
errno_exit("Failed to read format");
return;
}
fill_ctrls();
printf(" buffer parameters: %dx%d plane[0]=%d\n",
fmt.fmt.pix_mp.width, fmt.fmt.pix_mp.height,
fmt.fmt.pix_mp.plane_fmt[0].sizeimage);
printf("====== Calling S_CTRL ==================================\n");
for (i = 0; i < S5K_CTRL_NUM; i++) {
ret |= ioctl(g_file_desc, VIDIOC_S_CTRL, s5k_ctrl + i);
}
if (ret) {
errno_exit(" error S_CTRL");
}
printf("====== Calling G_CTRL ==================================\n");
for (i = 0; i < S5K_CTRL_NUM; i++) {
ret |= ioctl(g_file_desc, VIDIOC_G_CTRL, s5k_ctrl + i);
printf(" CTRL ID %d VAL %d \n", (s5k_ctrl + i)->id,
(s5k_ctrl + i)->value);
}
if (ret) {
errno_exit(" error G_CTRL");
}
}
static void init_v4l2_device(void)
{
struct v4l2_capability cap;
struct v4l2_cropcap cropcap;
struct v4l2_crop crop;
struct v4l2_format fmt;
if (-1 == xioctl(g_file_desc, VIDIOC_QUERYCAP, &cap)) {
if (EINVAL == errno) {
fprintf(stderr, "%s is no V4L2 device\n", dev_name);
exit(EXIT_FAILURE);
} else {
errno_exit("VIDIOC_QUERYCAP");
}
}
if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
fprintf(stderr, "%s is no video capture device\n", dev_name);
exit(EXIT_FAILURE);
}
if (!(cap.capabilities & V4L2_CAP_STREAMING)) {
fprintf(stderr, "%s does not support streaming i/o\n",
dev_name);
exit(EXIT_FAILURE);
}
#ifdef TEST_CROPCAP
CLEAR(cropcap);
cropcap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
PR_IO(VIDIOC_CROPCAP);
if (0 == xioctl(g_file_desc, VIDIOC_CROPCAP, &cropcap)) {
crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
crop.c = cropcap.defrect; /* reset to default */
if (-1 == xioctl(g_file_desc, VIDIOC_S_CROP, &crop)) {
switch (errno) {
case EINVAL:
/* Cropping not supported. */
break;
default:
/* Errors ignored. */
break;
}
}
} else {
/* Errors ignored. */
}
#endif
CLEAR(fmt);
fmt.fmt.pix_mp.num_planes = 1;
fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
fmt.fmt.pix_mp.width = g_pix_width;
fmt.fmt.pix_mp.height = g_pix_height;
fmt.fmt.pix_mp.pixelformat = V4L2_PIX_FMT_YUYV;
fmt.fmt.pix_mp.field = V4L2_FIELD_ANY;
PR_IO(VIDIOC_S_FMT);
if (-1 == xioctl(g_file_desc, VIDIOC_S_FMT, &fmt))
errno_exit("VIDIOC_S_FMT");
/* Note VIDIOC_S_FMT may change width and height. */
#if 0
/* Buggy driver paranoia. */
min = fmt.fmt.pix.width * 2;
if (fmt.fmt.pix.bytesperline < min)
fmt.fmt.pix.bytesperline = min;
min = fmt.fmt.pix.bytesperline * fmt.fmt.pix.height;
if (fmt.fmt.pix.sizeimage < min)
fmt.fmt.pix.sizeimage = min;
#endif
}
static int init_mmap(unsigned int *n_buffers)
{
struct v4l2_requestbuffers req;
unsigned int buf_index;
struct v4l2_plane planes[VIDEO_MAX_PLANES];
u32 offset, length;
memset(&req, 0, sizeof(req));
req.count = *n_buffers;
req.type = CAPTURE_BUFFER_TYPE;
req.memory = V4L2_MEMORY_MMAP;
PR_IO(VIDIOC_REQBUFS);
if (-1 == ioctl(g_file_desc, VIDIOC_REQBUFS, &req)) {
if (EINVAL == errno)
printf
("REQBUFS failed. No support for memory mapping?\n");
else
perror("VIDIOC_REQBUFS ioctl");
return -1;
}
if (req.count < 2) {
printf("Insufficient buffer memory\n");
return -1;
}
/* Number of buffers might got adjusted by driver so we propagate real
value up to the caller */
*n_buffers = req.count;
buffers = calloc(req.count, sizeof(*buffers));
if (!buffers) {
printf("Out of memory\n");
exit(EXIT_FAILURE);
}
PR_IO(VIDIOC_QUERYBUF);
for (buf_index = 0; buf_index < req.count; ++buf_index) {
struct v4l2_buffer buf;
memset(&buf, 0, sizeof(buf));
buf.type = CAPTURE_BUFFER_TYPE;
if (buf.type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
buf.m.planes = planes;
buf.length = 1; /* just one plane, depends on pixel format */
}
buf.memory = V4L2_MEMORY_MMAP;
buf.index = buf_index;
if (-1 == ioctl(g_file_desc, VIDIOC_QUERYBUF, &buf)) {
perror("VIDIOC_QUERYBUF");
return -1;
}
printf
("====== buf_index %d ==================================\n",
buf_index);
if (buf.type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
offset = buf.m.planes[0].m.mem_offset;
length = buf.m.planes[0].length;
} else {
offset = buf.m.offset;
length = buf.length;
}
printf("Plane offset: %d\n", offset);
printf("Plane length: %d\n", length);
buffers[buf_index].size[0] = length;
buffers[buf_index].addr[0] = mmap(NULL /* start anywhere */ ,
length, PROT_READ | PROT_WRITE
/* required */ ,
MAP_SHARED /* recommended */ ,
g_file_desc, offset);
if (MAP_FAILED == buffers[buf_index].addr[0]) {
perror("mmap");
return -1;
}
buffers[buf_index].index = buf_index;
buffers[buf_index].width = g_pix_width;
buffers[buf_index].height = g_pix_height;
printf("mmaped: buf_index: %d, size: %ld, addr: %p\n",
buf_index, buffers[buf_index].size[0],
buffers[buf_index].addr[0]);
}
return 0;
}