static void mxc_v4l2out_timer_handler(unsigned long arg)
{
int index;
unsigned long timeout;
vout_data *vout = (vout_data *) arg;
/* DPRINTK("timer handler:\n"); */
/* If timer occurs before IPU h/w is ready, then set the state to
paused and the timer will be set again when next buffer is queued. */
if (vout->ipu_buf[vout->next_rdy_ipu_buf] != -1) {
DPRINTK("IPU buffer busy\n");
vout->state = STATE_STREAM_PAUSED;
return;
}
/* Dequeue buffer and pass to IPU */
index = dequeue_buf(&vout->ready_q);
if (index == -1) { /* no buffers ready, should never occur */
printk("mxc_v4l2out: timer - no queued buffers ready\n");
return;
}
g_buf_dq_cnt++;
vout->ipu_buf[vout->next_rdy_ipu_buf] = index;
if (ipu_update_channel_buffer(vout->post_proc_ch, IPU_INPUT_BUFFER,
vout->next_rdy_ipu_buf,
vout->queue_buf_paddr[index]) < 0) {
DPRINTK("unable to update buffer %d address\n",
vout->next_rdy_ipu_buf);
return;
}
if (ipu_select_buffer(vout->post_proc_ch, IPU_INPUT_BUFFER,
vout->next_rdy_ipu_buf) < 0) {
DPRINTK("unable to set IPU buffer ready\n");
}
vout->next_rdy_ipu_buf = !vout->next_rdy_ipu_buf;
/* Setup timer for next buffer */
index = peek_next_buf(&vout->ready_q);
if (index != -1) {
timeout = timeval_to_jiffies(&vout->v4l2_bufs[index].timestamp);
if (!timeout) {
/* if timestamp is 0, then default to 30fps */
timeout = vout->start_jiffies +
msecs_to_jiffies(vout->frame_count * 33);
} else { /* Adjust time from time of day to jiffies */
timeout -= vout->start_tod_jiffies;
}
if (mod_timer(&vout->output_timer, timeout))
DPRINTK("warning: timer was already set\n");
vout->frame_count++;
} else {
vout->state = STATE_STREAM_PAUSED;
}
}
void hw_camdrv_msleep(unsigned int ms)
{
struct timeval now;
unsigned long jiffies;
if (ms > 0) {
now.tv_sec = ms / 1000;
now.tv_usec = (ms % 1000) * 1000;
jiffies = timeval_to_jiffies(&now);
schedule_timeout_interruptible(jiffies);
}
}
int do_setitimer(int which, struct itimerval *value, struct itimerval *ovalue)
{
register unsigned long i, j;
int k;
i = timeval_to_jiffies(&value->it_interval);
j = timeval_to_jiffies(&value->it_value);
if (ovalue && (k = do_getitimer(which, ovalue)) < 0)
return k;
ltt_ev_timer(LTT_EV_TIMER_SETITIMER, which, i, j);
switch (which) {
case ITIMER_REAL:
del_timer_sync(¤t->real_timer);
current->it_real_value = j;
current->it_real_incr = i;
if (!j)
break;
if (j > (unsigned long) LONG_MAX)
j = LONG_MAX;
i = j + jiffies;
current->real_timer.expires = i;
add_timer(¤t->real_timer);
break;
case ITIMER_VIRTUAL:
if (j)
j++;
current->it_virt_value = j;
current->it_virt_incr = i;
break;
case ITIMER_PROF:
if (j)
j++;
current->it_prof_value = j;
current->it_prof_incr = i;
break;
default:
return -EINVAL;
}
return 0;
}
static void sync_cmos_clock(unsigned long dummy)
{
struct timeval now, next;
int fail = 1;
/*
* If we have an externally synchronized Linux clock, then update
* CMOS clock accordingly every ~11 minutes. Set_rtc_mmss() has to be
* called as close as possible to 500 ms before the new second starts.
* This code is run on a timer. If the clock is set, that timer
* may not expire at the correct time. Thus, we adjust...
*/
if (!ntp_synced())
/*
* Not synced, exit, do not restart a timer (if one is
* running, let it run out).
*/
return;
do_gettimeofday(&now);
if (now.tv_usec >= USEC_AFTER - ((unsigned) TICK_SIZE) / 2 &&
now.tv_usec <= USEC_BEFORE + ((unsigned) TICK_SIZE) / 2)
fail = set_rtc_mmss(now.tv_sec);
next.tv_usec = USEC_AFTER - now.tv_usec;
if (next.tv_usec <= 0)
next.tv_usec += USEC_PER_SEC;
if (!fail)
next.tv_sec = 659;
else
next.tv_sec = 0;
if (next.tv_usec >= USEC_PER_SEC) {
next.tv_sec++;
next.tv_usec -= USEC_PER_SEC;
}
mod_timer(&sync_cmos_timer, jiffies + timeval_to_jiffies(&next));
}
static __inline unsigned long get_jiffies(struct timeval *t)
{
struct timeval cur;
if (t->tv_usec >= 1000000) {
t->tv_sec += t->tv_usec / 1000000;
t->tv_usec = t->tv_usec % 1000000;
}
do_gettimeofday(&cur);
if ((t->tv_sec < cur.tv_sec)
|| ((t->tv_sec == cur.tv_sec) && (t->tv_usec < cur.tv_usec)))
return jiffies;
if (t->tv_usec < cur.tv_usec) {
cur.tv_sec = t->tv_sec - cur.tv_sec - 1;
cur.tv_usec = t->tv_usec + 1000000 - cur.tv_usec;
} else {
cur.tv_sec = t->tv_sec - cur.tv_sec;
cur.tv_usec = t->tv_usec - cur.tv_usec;
}
return jiffies + timeval_to_jiffies(&cur);
}
/*!
* V4L2 interface - ioctl function
*
* @param inode struct inode *
*
* @param file struct file *
*
* @param ioctlnr unsigned int
*
* @param arg void *
*
* @return 0 success, ENODEV for invalid device instance,
* -1 for other errors.
*/
static int
mxc_v4l2out_do_ioctl(struct inode *inode, struct file *file,
unsigned int ioctlnr, void *arg)
{
struct video_device *dev = file->private_data;
vout_data *vout = video_get_drvdata(dev);
int retval = 0;
int i = 0;
if (!vout)
return -ENODEV;
/* make this _really_ smp-safe */
if (down_interruptible(&vout->busy_lock))
return -EINTR;
switch (ioctlnr) {
case VIDIOC_QUERYCAP:
{
struct v4l2_capability *cap = arg;
strcpy(cap->driver, "mxc_v4l2_output");
cap->version = 0;
cap->capabilities =
V4L2_CAP_VIDEO_OUTPUT | V4L2_CAP_STREAMING;
retval = 0;
break;
}
case VIDIOC_G_FMT:
{
struct v4l2_format *gf = arg;
retval = mxc_v4l2out_g_fmt(vout, gf);
break;
}
case VIDIOC_S_FMT:
{
struct v4l2_format *sf = arg;
if (vout->state != STATE_STREAM_OFF) {
retval = -EBUSY;
break;
}
retval = mxc_v4l2out_s_fmt(vout, sf);
break;
}
case VIDIOC_REQBUFS:
{
struct v4l2_requestbuffers *req = arg;
if ((req->type != V4L2_BUF_TYPE_VIDEO_OUTPUT) ||
(req->memory != V4L2_MEMORY_MMAP)) {
DPRINTK
("VIDIOC_REQBUFS: incorrect buffer type\n");
retval = -EINVAL;
break;
}
if (req->count == 0) {
mxc_v4l2out_streamoff(vout);
if (vout->queue_buf_paddr[0] != 0) {
mxc_free_buffers(vout->queue_buf_paddr,
vout->buffer_cnt);
DPRINTK
("VIDIOC_REQBUFS: freed buffers\n");
}
vout->buffer_cnt = 0;
break;
}
if (vout->queue_buf_paddr[0] != 0) {
DPRINTK
("VIDIOC_REQBUFS: Cannot allocate buffers\n");
retval = -EBUSY;
break;
}
if (req->count < MIN_FRAME_NUM) {
req->count = MIN_FRAME_NUM;
} else if (req->count > MAX_FRAME_NUM) {
req->count = MAX_FRAME_NUM;
}
vout->buffer_cnt = req->count;
retval = mxc_allocate_buffers(vout->queue_buf_paddr,
vout->buffer_cnt,
PAGE_ALIGN(vout->v2f.fmt.
pix.
sizeimage));
if (retval < 0)
break;
/* Init buffer queues */
vout->done_q.head = 0;
vout->done_q.tail = 0;
vout->ready_q.head = 0;
vout->ready_q.tail = 0;
for (i = 0; i < vout->buffer_cnt; i++) {
memset(&(vout->v4l2_bufs[i]), 0,
sizeof(vout->v4l2_bufs[i]));
vout->v4l2_bufs[i].flags = 0;
vout->v4l2_bufs[i].memory = V4L2_MEMORY_MMAP;
vout->v4l2_bufs[i].index = i;
vout->v4l2_bufs[i].type =
V4L2_BUF_TYPE_VIDEO_OUTPUT;
vout->v4l2_bufs[i].length =
PAGE_ALIGN(vout->v2f.fmt.pix.sizeimage);
vout->v4l2_bufs[i].m.offset =
(unsigned long)vout->queue_buf_paddr[i];
vout->v4l2_bufs[i].timestamp.tv_sec = 0;
vout->v4l2_bufs[i].timestamp.tv_usec = 0;
}
break;
}
case VIDIOC_QUERYBUF:
{
struct v4l2_buffer *buf = arg;
u32 type = buf->type;
int index = buf->index;
if ((type != V4L2_BUF_TYPE_VIDEO_OUTPUT) ||
(index >= vout->buffer_cnt)) {
DPRINTK
("VIDIOC_QUERYBUFS: incorrect buffer type\n");
retval = -EINVAL;
break;
}
down(&vout->param_lock);
memcpy(buf, &(vout->v4l2_bufs[index]), sizeof(*buf));
up(&vout->param_lock);
break;
}
case VIDIOC_QBUF:
{
struct v4l2_buffer *buf = arg;
int index = buf->index;
u32 lock_flags;
if ((buf->type != V4L2_BUF_TYPE_VIDEO_OUTPUT) ||
(index >= vout->buffer_cnt) || (buf->flags != 0)) {
retval = -EINVAL;
break;
}
DPRINTK("VIDIOC_QBUF: %d\n", buf->index);
spin_lock_irqsave(&g_lock, lock_flags);
memcpy(&(vout->v4l2_bufs[index]), buf, sizeof(*buf));
vout->v4l2_bufs[index].flags |= V4L2_BUF_FLAG_QUEUED;
g_buf_q_cnt++;
queue_buf(&vout->ready_q, index);
if (vout->state == STATE_STREAM_PAUSED) {
unsigned long timeout =
timeval_to_jiffies(&vout->v4l2_bufs[index].
timestamp);
if (!timeout) {
/* if timestamp is 0, then default to 30fps */
timeout = vout->start_jiffies +
msecs_to_jiffies(vout->frame_count *
33);
} else { /* Adjust time from time of day to jiffies */
timeout -= vout->start_tod_jiffies;
}
vout->output_timer.expires = timeout;
DPRINTK("QBUF: frame #%u timeout @ %u jiffies, "
"current = %u\n",
vout->frame_count, timeout, jiffies);
add_timer(&vout->output_timer);
vout->state = STATE_STREAM_ON;
vout->frame_count++;
}
spin_unlock_irqrestore(&g_lock, lock_flags);
break;
}
case VIDIOC_DQBUF:
{
struct v4l2_buffer *buf = arg;
int idx;
/* DPRINTK("VIDIOC_DQBUF: q size = %d\n",
queue_size(&vout->done_q));
*/
if ((queue_size(&vout->done_q) == 0) &&
(file->f_flags & O_NONBLOCK)) {
retval = -EAGAIN;
break;
}
if (!wait_event_interruptible_timeout(vout->v4l_bufq,
queue_size(&vout->
done_q)
!= 0, 2 * HZ)) {
printk("VIDIOC_DQBUF: timeout\n");
retval = -ETIME;
break;
} else if (signal_pending(current)) {
printk("VIDIOC_DQBUF: interrupt received\n");
vout->state = STATE_STREAM_STOPPING;
retval = -ERESTARTSYS;
break;
}
idx = dequeue_buf(&vout->done_q);
if (idx == -1) { /* No frame free */
printk
("VIDIOC_DQBUF: no free buffers, returning\n");
retval = -EAGAIN;
break;
}
if ((vout->v4l2_bufs[idx].flags & V4L2_BUF_FLAG_DONE) ==
0)
printk
("VIDIOC_DQBUF: buffer in done q, but not "
"flagged as done\n");
vout->v4l2_bufs[idx].flags = 0;
memcpy(buf, &(vout->v4l2_bufs[idx]), sizeof(*buf));
DPRINTK("VIDIOC_DQBUF: %d\n", buf->index);
break;
}
case VIDIOC_STREAMON:
{
struct timeval t;
do_gettimeofday(&t);
vout->start_tod_jiffies =
timeval_to_jiffies(&t) - jiffies;
vout->frame_count = 2;
vout->start_jiffies = jiffies;
DPRINTK("VIDIOC_STREAMON: start time = %u jiffies, "
"tod adjustment = %u\n",
vout->start_jiffies, vout->start_tod_jiffies);
retval = mxc_v4l2out_streamon(vout);
break;
}
case VIDIOC_STREAMOFF:
{
retval = mxc_v4l2out_streamoff(vout);
break;
}
case VIDIOC_G_CTRL:
{
retval = mxc_get_v42lout_control(vout, arg);
break;
}
case VIDIOC_S_CTRL:
{
retval = mxc_set_v42lout_control(vout, arg);
break;
}
case VIDIOC_CROPCAP:
{
struct v4l2_cropcap *cap = arg;
if (cap->type != V4L2_BUF_TYPE_VIDEO_OUTPUT) {
retval = -EINVAL;
break;
}
cap->bounds = vout->crop_bounds[vout->cur_disp_output];
cap->defrect = vout->crop_bounds[vout->cur_disp_output];
retval = 0;
break;
}
case VIDIOC_G_CROP:
{
struct v4l2_crop *crop = arg;
if (crop->type != V4L2_BUF_TYPE_VIDEO_OUTPUT) {
retval = -EINVAL;
break;
}
crop->c = vout->crop_current;
break;
}
case VIDIOC_S_CROP:
{
struct v4l2_crop *crop = arg;
struct v4l2_rect *b =
&(vout->crop_bounds[vout->cur_disp_output]);
if (crop->type != V4L2_BUF_TYPE_VIDEO_OUTPUT) {
retval = -EINVAL;
break;
}
if (crop->c.height < 0) {
retval = -EINVAL;
break;
}
if (crop->c.width < 0) {
retval = -EINVAL;
break;
}
if (crop->c.top < b->top)
crop->c.top = b->top;
if (crop->c.top > b->top + b->height)
crop->c.top = b->top + b->height;
if (crop->c.height > b->top - crop->c.top + b->height)
crop->c.height =
b->top - crop->c.top + b->height;
if (crop->c.left < b->left)
crop->c.top = b->left;
if (crop->c.left > b->left + b->width)
crop->c.top = b->left + b->width;
if (crop->c.width > b->left - crop->c.left + b->width)
crop->c.width =
b->left - crop->c.left + b->width;
/* stride line limitation */
crop->c.height -= crop->c.height % 8;
crop->c.width -= crop->c.width % 8;
vout->crop_current = crop->c;
vout->sdc_fg_buf_size = vout->crop_current.width *
vout->crop_current.height;
vout->sdc_fg_buf_size *=
fmt_to_bpp(SDC_FG_FB_FORMAT) / 8;
/* Free previously allocated buffer */
if (vout->display_bufs[0] != NULL) {
mxc_free_buffers(vout->display_bufs, 2);
}
if ((retval = mxc_allocate_buffers(vout->display_bufs,
2,
vout->
sdc_fg_buf_size)) <
0) {
DPRINTK("unable to allocate SDC FG buffers\n");
retval = -ENOMEM;
break;
}
break;
}
case VIDIOC_ENUMOUTPUT:
{
struct v4l2_output *output = arg;
if ((output->index >= 4) ||
(vout->output_enabled[output->index] == false)) {
retval = -EINVAL;
break;
}
if (output->index < 3) {
*output = mxc_outputs[MXC_V4L2_OUT_2_ADC];
output->name[4] = '0' + output->index;
} else {
*output = mxc_outputs[MXC_V4L2_OUT_2_SDC];
}
break;
}
case VIDIOC_G_OUTPUT:
{
int *p_output_num = arg;
*p_output_num = vout->cur_disp_output;
break;
}
case VIDIOC_S_OUTPUT:
{
int *p_output_num = arg;
if ((*p_output_num >= 4) ||
(vout->output_enabled[*p_output_num] == false)) {
retval = -EINVAL;
break;
}
if (vout->state != STATE_STREAM_OFF) {
retval = -EBUSY;
break;
}
vout->cur_disp_output = *p_output_num;
break;
}
case VIDIOC_ENUM_FMT:
case VIDIOC_TRY_FMT:
case VIDIOC_QUERYCTRL:
case VIDIOC_G_PARM:
case VIDIOC_ENUMSTD:
case VIDIOC_G_STD:
case VIDIOC_S_STD:
case VIDIOC_G_TUNER:
case VIDIOC_S_TUNER:
case VIDIOC_G_FREQUENCY:
case VIDIOC_S_FREQUENCY:
default:
retval = -EINVAL;
break;
}
up(&vout->busy_lock);
return retval;
}
static void
ssh_interceptor_dst_entry_cache_timeout(unsigned long data)
{
SshInterceptor interceptor = ssh_interceptor_context;
SshUInt32 slot;
SshDstEntry tmp, prev = NULL;
struct timeval tv;
unsigned long time_now;
unsigned long expiry;
ssh_kernel_mutex_lock(interceptor->dst_entry_cache_lock);
SSH_DEBUG(SSH_D_MIDOK,
("Dst entry cache timeout %lu items in cache",
(unsigned long)interceptor->dst_entry_cached_items));
SSH_ASSERT(interceptor->dst_entry_cache_timeout_registered == TRUE);
if (interceptor->dst_entry_cached_items == 0)
{
interceptor->dst_entry_cache_timeout_registered = FALSE;
ssh_kernel_mutex_unlock(interceptor->dst_entry_cache_lock);
return;
}
tv.tv_sec = DST_ENTRY_MAX_CACHE_TIME;
tv.tv_usec = 0;
time_now = jiffies;
expiry = timeval_to_jiffies(&tv);
for (slot = 0; slot < SSH_DST_ENTRY_TBL_SIZE; slot++)
{
restart:
prev = NULL;
for (tmp = interceptor->dst_entry_table[slot];
tmp != NULL;
tmp = tmp->next)
{
/* Do we have a match? */
if ((tmp->allocation_time + expiry) < time_now ||
(time_now - tmp->allocation_time) > expiry)
{
/* Head of list. */
if (tmp == interceptor->dst_entry_table[slot])
{
SSH_DEBUG(SSH_D_MIDOK,
("Dst entry cache timeout freeing head ID %lu",
(unsigned long)tmp->dst_entry_id));
interceptor->dst_entry_table[slot] = tmp->next;
interceptor->dst_entry_cached_items--;
dst_release(tmp->dst_entry);
ssh_free(tmp);
goto restart;
}
/* Any other place in the list. */
else
{
prev->next = tmp->next;
interceptor->dst_entry_cached_items--;
SSH_DEBUG(SSH_D_MIDOK,
("Dst entry cache timeout freeing ID %lu",
(unsigned long)tmp->dst_entry_id));
dst_release(tmp->dst_entry);
ssh_free(tmp);
goto restart;
}
}
prev = tmp;
}
}
if (interceptor->dst_entry_cached_items > 0)
{
struct timeval tv;
tv.tv_sec = DST_ENTRY_MAX_CACHE_TIME;
tv.tv_usec = 0;
interceptor->dst_cache_timer.expires = jiffies + timeval_to_jiffies(&tv);
interceptor->dst_cache_timer.data = (unsigned long)interceptor;
interceptor->dst_cache_timer.function =
ssh_interceptor_dst_entry_cache_timeout;
mod_timer(&interceptor->dst_cache_timer,
interceptor->dst_cache_timer.expires);
}
else
{
interceptor->dst_entry_cache_timeout_registered = FALSE;
}
SSH_DEBUG(SSH_D_NICETOKNOW, ("Left %lu items in dst cache",
(unsigned long)
interceptor->dst_entry_cached_items));
ssh_kernel_mutex_unlock(interceptor->dst_entry_cache_lock);
}
/* Cache a dst entry for later purposes. This is required by the
pass unmodified to work. If we lose the dst entry, we basically
cannot return the packet as unmodified to the linux. Return 0
if the caching fails. If it succeeds, return a valid cache ID. */
SshUInt32
ssh_interceptor_packet_cache_dst_entry(SshInterceptor interceptor,
SshInterceptorPacket pp)
{
SshInterceptorInternalPacket ipp = (SshInterceptorInternalPacket)pp;
SshDstEntry cache_dst;
SshDstEntry tmp;
SshUInt32 slot;
SSH_DEBUG(SSH_D_MIDOK,
("Dst entry cache, caching dst for pp 0x%p, %lu items in cache",
pp, (unsigned long)interceptor->dst_entry_cached_items));
if (ipp->skb == NULL || SSH_SKB_DST(ipp->skb) == NULL)
return 0;
cache_dst = ssh_calloc(1, sizeof(SshDstEntryStruct));
if (cache_dst == NULL)
return 0;
cache_dst->allocation_time = jiffies;
cache_dst->next = NULL;
cache_dst->dst_entry = SSH_SKB_DST(ipp->skb);
dst_hold(cache_dst->dst_entry);
ssh_kernel_mutex_lock(interceptor->dst_entry_cache_lock);
cache_dst->dst_entry_id = interceptor->dst_entry_id++;
slot = cache_dst->dst_entry_id % SSH_DST_ENTRY_TBL_SIZE;
interceptor->dst_entry_cached_items++;
SSH_ASSERT(slot < SSH_DST_ENTRY_TBL_SIZE);
/* Head of list. */
if (interceptor->dst_entry_table[slot] == NULL)
{
interceptor->dst_entry_table[slot] = cache_dst;
}
else
{
/* We do not care about potential collisions. These are highly unlikely
to happen and in the end */
for (tmp = interceptor->dst_entry_table[slot];
tmp->next != NULL;
tmp = tmp->next)
SSH_ASSERT(cache_dst->dst_entry_id != tmp->dst_entry_id);
tmp->next = cache_dst;
}
/* Handle special case, the id is overflowing. 0 is used for special
purposes, i.e. for 'real' engine created packets. */
if (interceptor->dst_entry_id == 0)
interceptor->dst_entry_id = 1;
if (interceptor->dst_entry_cache_timeout_registered == FALSE)
{
struct timeval tv;
SSH_ASSERT(interceptor->dst_entry_cached_items > 0);
tv.tv_sec = DST_ENTRY_MAX_CACHE_TIME;
tv.tv_usec = 0;
init_timer(&interceptor->dst_cache_timer);
interceptor->dst_cache_timer.expires = jiffies + timeval_to_jiffies(&tv);
interceptor->dst_cache_timer.data = (unsigned long)interceptor;
interceptor->dst_cache_timer.function =
ssh_interceptor_dst_entry_cache_timeout;
add_timer(&interceptor->dst_cache_timer);
interceptor->dst_entry_cache_timeout_registered = TRUE;
}
SSH_DEBUG(SSH_D_NICETOKNOW, ("Cache ID %lu, left %lu items in dst cache",
(unsigned long)cache_dst->dst_entry_id,
(unsigned long)
interceptor->dst_entry_cached_items));
ssh_kernel_mutex_unlock(interceptor->dst_entry_cache_lock);
return cache_dst->dst_entry_id;
}
