int solo_v4l2_init(struct solo6010_dev *solo_dev)
{
int ret;
int i;
init_waitqueue_head(&solo_dev->disp_thread_wait);
solo_dev->vfd = video_device_alloc();
if (!solo_dev->vfd)
return -ENOMEM;
*solo_dev->vfd = solo_v4l2_template;
solo_dev->vfd->parent = &solo_dev->pdev->dev;
ret = video_register_device(solo_dev->vfd, VFL_TYPE_GRABBER, video_nr);
if (ret < 0) {
video_device_release(solo_dev->vfd);
solo_dev->vfd = NULL;
return ret;
}
video_set_drvdata(solo_dev->vfd, solo_dev);
snprintf(solo_dev->vfd->name, sizeof(solo_dev->vfd->name), "%s (%i)",
SOLO6010_NAME, solo_dev->vfd->num);
if (video_nr != -1)
video_nr++;
dev_info(&solo_dev->pdev->dev, "Display as /dev/video%d with "
"%d inputs (%d extended)\n", solo_dev->vfd->num,
solo_dev->nr_chans, solo_dev->nr_ext);
/* Cycle all the channels and clear */
for (i = 0; i < solo_dev->nr_chans; i++) {
solo_v4l2_set_ch(solo_dev, i);
while (erase_off(solo_dev))
;// Do nothing
}
/* Set the default display channel */
solo_v4l2_set_ch(solo_dev, 0);
while (erase_off(solo_dev))
;// Do nothing
solo6010_irq_on(solo_dev, SOLO_IRQ_VIDEO_IN);
return 0;
}
static void solo_fillbuf(struct solo_filehandle *fh,
struct videobuf_buffer *vb)
{
struct solo6010_dev *solo_dev = fh->solo_dev;
dma_addr_t vbuf;
unsigned int fdma_addr;
int frame_size;
int error = 1;
int i;
if (!(vbuf = videobuf_to_dma_contig(vb)))
goto finish_buf;
if (erase_off(solo_dev)) {
void *p = videobuf_queue_to_vaddr(&fh->vidq, vb);
int image_size = solo_image_size(solo_dev);
for (i = 0; i < image_size; i += 2) {
((u8 *)p)[i] = 0x80;
((u8 *)p)[i + 1] = 0x00;
}
error = 0;
goto finish_buf;
}
frame_size = SOLO_HW_BPL * solo_vlines(solo_dev);
fdma_addr = SOLO_DISP_EXT_ADDR(solo_dev) + (fh->old_write * frame_size);
for (i = 0; i < frame_size / SOLO_DISP_BUF_SIZE; i++) {
int j;
for (j = 0; j < (SOLO_DISP_BUF_SIZE / SOLO_HW_BPL); j++) {
if (solo_p2m_dma_t(solo_dev, SOLO_P2M_DMA_ID_DISP, 0,
vbuf, fdma_addr + (j * SOLO_HW_BPL),
solo_bytesperline(solo_dev)))
goto finish_buf;
vbuf += solo_bytesperline(solo_dev);
}
fdma_addr += SOLO_DISP_BUF_SIZE;
}
error = 0;
finish_buf:
if (error) {
vb->state = VIDEOBUF_ERROR;
} else {
vb->state = VIDEOBUF_DONE;
vb->field_count++;
do_gettimeofday(&vb->ts);
}
wake_up(&vb->done);
return;
}
static int solo_set_input(struct file *file, void *priv, unsigned int index)
{
struct solo_filehandle *fh = priv;
int ret = solo_v4l2_set_ch(fh->solo_dev, index);
if (!ret) {
while (erase_off(fh->solo_dev))
/* Do nothing */;
}
return ret;
}
static int solo_set_input(struct file *file, void *priv, unsigned int index)
{
struct solo_dev *solo_dev = video_drvdata(file);
int ret = solo_v4l2_set_ch(solo_dev, index);
if (!ret) {
while (erase_off(solo_dev))
/* Do nothing */;
}
return ret;
}
static void solo_fillbuf(struct solo_dev *solo_dev,
struct vb2_buffer *vb)
{
struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
dma_addr_t addr;
unsigned int fdma_addr;
int error = -1;
int i;
addr = vb2_dma_contig_plane_dma_addr(vb, 0);
if (!addr)
goto finish_buf;
if (erase_off(solo_dev)) {
void *p = vb2_plane_vaddr(vb, 0);
int image_size = solo_image_size(solo_dev);
for (i = 0; i < image_size; i += 2) {
((u8 *)p)[i] = 0x80;
((u8 *)p)[i + 1] = 0x00;
}
error = 0;
} else {
fdma_addr = SOLO_DISP_EXT_ADDR + (solo_dev->old_write *
(SOLO_HW_BPL * solo_vlines(solo_dev)));
error = solo_p2m_dma_t(solo_dev, 0, addr, fdma_addr,
solo_bytesperline(solo_dev),
solo_vlines(solo_dev), SOLO_HW_BPL);
}
finish_buf:
if (!error) {
vb2_set_plane_payload(vb, 0,
solo_vlines(solo_dev) * solo_bytesperline(solo_dev));
vbuf->sequence = solo_dev->sequence++;
vb->timestamp = ktime_get_ns();
}
vb2_buffer_done(vb, error ? VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE);
}
static void solo_fillbuf(struct solo_filehandle *fh,
struct videobuf_buffer *vb)
{
struct solo_dev *solo_dev = fh->solo_dev;
dma_addr_t vbuf;
unsigned int fdma_addr;
int error = -1;
int i;
vbuf = videobuf_to_dma_contig(vb);
if (!vbuf)
goto finish_buf;
if (erase_off(solo_dev)) {
void *p = videobuf_queue_to_vaddr(&fh->vidq, vb);
int image_size = solo_image_size(solo_dev);
for (i = 0; i < image_size; i += 2) {
((u8 *)p)[i] = 0x80;
((u8 *)p)[i + 1] = 0x00;
}
error = 0;
} else {
fdma_addr = SOLO_DISP_EXT_ADDR + (fh->old_write *
(SOLO_HW_BPL * solo_vlines(solo_dev)));
error = solo_p2m_dma_t(solo_dev, 0, vbuf, fdma_addr,
solo_bytesperline(solo_dev),
solo_vlines(solo_dev), SOLO_HW_BPL);
}
finish_buf:
if (error) {
vb->state = VIDEOBUF_ERROR;
} else {
vb->state = VIDEOBUF_DONE;
vb->field_count++;
}
wake_up(&vb->done);
}
int solo_v4l2_init(struct solo_dev *solo_dev, unsigned nr)
{
int ret;
int i;
init_waitqueue_head(&solo_dev->disp_thread_wait);
spin_lock_init(&solo_dev->slock);
mutex_init(&solo_dev->lock);
INIT_LIST_HEAD(&solo_dev->vidq_active);
solo_dev->vfd = video_device_alloc();
if (!solo_dev->vfd)
return -ENOMEM;
*solo_dev->vfd = solo_v4l2_template;
solo_dev->vfd->v4l2_dev = &solo_dev->v4l2_dev;
solo_dev->vfd->queue = &solo_dev->vidq;
solo_dev->vfd->lock = &solo_dev->lock;
v4l2_ctrl_handler_init(&solo_dev->disp_hdl, 1);
v4l2_ctrl_new_custom(&solo_dev->disp_hdl, &solo_motion_trace_ctrl, NULL);
if (solo_dev->disp_hdl.error) {
ret = solo_dev->disp_hdl.error;
goto fail;
}
solo_dev->vfd->ctrl_handler = &solo_dev->disp_hdl;
video_set_drvdata(solo_dev->vfd, solo_dev);
solo_dev->vidq.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
solo_dev->vidq.io_modes = VB2_MMAP | VB2_USERPTR | VB2_READ;
solo_dev->vidq.ops = &solo_video_qops;
solo_dev->vidq.mem_ops = &vb2_dma_contig_memops;
solo_dev->vidq.drv_priv = solo_dev;
solo_dev->vidq.timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
solo_dev->vidq.gfp_flags = __GFP_DMA32;
solo_dev->vidq.buf_struct_size = sizeof(struct solo_vb2_buf);
solo_dev->vidq.lock = &solo_dev->lock;
ret = vb2_queue_init(&solo_dev->vidq);
if (ret < 0)
goto fail;
solo_dev->alloc_ctx = vb2_dma_contig_init_ctx(&solo_dev->pdev->dev);
if (IS_ERR(solo_dev->alloc_ctx)) {
dev_err(&solo_dev->pdev->dev, "Can't allocate buffer context");
return PTR_ERR(solo_dev->alloc_ctx);
}
/* Cycle all the channels and clear */
for (i = 0; i < solo_dev->nr_chans; i++) {
solo_v4l2_set_ch(solo_dev, i);
while (erase_off(solo_dev))
/* Do nothing */;
}
/* Set the default display channel */
solo_v4l2_set_ch(solo_dev, 0);
while (erase_off(solo_dev))
/* Do nothing */;
ret = video_register_device(solo_dev->vfd, VFL_TYPE_GRABBER, nr);
if (ret < 0)
goto fail;
snprintf(solo_dev->vfd->name, sizeof(solo_dev->vfd->name), "%s (%i)",
SOLO6X10_NAME, solo_dev->vfd->num);
dev_info(&solo_dev->pdev->dev, "Display as /dev/video%d with "
"%d inputs (%d extended)\n", solo_dev->vfd->num,
solo_dev->nr_chans, solo_dev->nr_ext);
return 0;
fail:
video_device_release(solo_dev->vfd);
vb2_dma_contig_cleanup_ctx(solo_dev->alloc_ctx);
v4l2_ctrl_handler_free(&solo_dev->disp_hdl);
solo_dev->vfd = NULL;
return ret;
}
