cachepc-linux

Fork of AMDESE/linux with modifications for CachePC side-channel attack
git clone https://git.sinitax.com/sinitax/cachepc-linux
Log | Files | Refs | README | LICENSE | sfeed.txt

camss-vfe-gen1.c (18629B)


      1// SPDX-License-Identifier: GPL-2.0
      2/*
      3 * camss-vfe-gen1.c
      4 *
      5 * Qualcomm MSM Camera Subsystem - VFE Common functionality for Gen 1 versions of hw (4.1, 4.7..)
      6 *
      7 * Copyright (C) 2020 Linaro Ltd.
      8 */
      9
     10#include "camss.h"
     11#include "camss-vfe.h"
     12#include "camss-vfe-gen1.h"
     13
     14/* Max number of frame drop updates per frame */
     15#define VFE_FRAME_DROP_UPDATES 2
     16#define VFE_NEXT_SOF_MS 500
     17
     18int vfe_gen1_halt(struct vfe_device *vfe)
     19{
     20	unsigned long time;
     21
     22	reinit_completion(&vfe->halt_complete);
     23
     24	vfe->ops_gen1->halt_request(vfe);
     25
     26	time = wait_for_completion_timeout(&vfe->halt_complete,
     27					   msecs_to_jiffies(VFE_HALT_TIMEOUT_MS));
     28	if (!time) {
     29		dev_err(vfe->camss->dev, "VFE halt timeout\n");
     30		return -EIO;
     31	}
     32
     33	return 0;
     34}
     35
     36static int vfe_disable_output(struct vfe_line *line)
     37{
     38	struct vfe_device *vfe = to_vfe(line);
     39	struct vfe_output *output = &line->output;
     40	const struct vfe_hw_ops *ops = vfe->ops;
     41	unsigned long flags;
     42	unsigned long time;
     43	unsigned int i;
     44
     45	spin_lock_irqsave(&vfe->output_lock, flags);
     46
     47	output->gen1.wait_sof = 1;
     48	spin_unlock_irqrestore(&vfe->output_lock, flags);
     49
     50	time = wait_for_completion_timeout(&output->sof, msecs_to_jiffies(VFE_NEXT_SOF_MS));
     51	if (!time)
     52		dev_err(vfe->camss->dev, "VFE sof timeout\n");
     53
     54	spin_lock_irqsave(&vfe->output_lock, flags);
     55	for (i = 0; i < output->wm_num; i++)
     56		vfe->ops_gen1->wm_enable(vfe, output->wm_idx[i], 0);
     57
     58	ops->reg_update(vfe, line->id);
     59	output->wait_reg_update = 1;
     60	spin_unlock_irqrestore(&vfe->output_lock, flags);
     61
     62	time = wait_for_completion_timeout(&output->reg_update, msecs_to_jiffies(VFE_NEXT_SOF_MS));
     63	if (!time)
     64		dev_err(vfe->camss->dev, "VFE reg update timeout\n");
     65
     66	spin_lock_irqsave(&vfe->output_lock, flags);
     67
     68	if (line->id != VFE_LINE_PIX) {
     69		vfe->ops_gen1->wm_frame_based(vfe, output->wm_idx[0], 0);
     70		vfe->ops_gen1->bus_disconnect_wm_from_rdi(vfe, output->wm_idx[0], line->id);
     71		vfe->ops_gen1->enable_irq_wm_line(vfe, output->wm_idx[0], line->id, 0);
     72		vfe->ops_gen1->set_cgc_override(vfe, output->wm_idx[0], 0);
     73		spin_unlock_irqrestore(&vfe->output_lock, flags);
     74	} else {
     75		for (i = 0; i < output->wm_num; i++) {
     76			vfe->ops_gen1->wm_line_based(vfe, output->wm_idx[i], NULL, i, 0);
     77			vfe->ops_gen1->set_cgc_override(vfe, output->wm_idx[i], 0);
     78		}
     79
     80		vfe->ops_gen1->enable_irq_pix_line(vfe, 0, line->id, 0);
     81		vfe->ops_gen1->set_module_cfg(vfe, 0);
     82		vfe->ops_gen1->set_realign_cfg(vfe, line, 0);
     83		vfe->ops_gen1->set_xbar_cfg(vfe, output, 0);
     84		vfe->ops_gen1->set_camif_cmd(vfe, 0);
     85
     86		spin_unlock_irqrestore(&vfe->output_lock, flags);
     87
     88		vfe->ops_gen1->camif_wait_for_stop(vfe, vfe->camss->dev);
     89	}
     90
     91	return 0;
     92}
     93
     94/*
     95 * vfe_gen1_disable - Disable streaming on VFE line
     96 * @line: VFE line
     97 *
     98 * Return 0 on success or a negative error code otherwise
     99 */
    100int vfe_gen1_disable(struct vfe_line *line)
    101{
    102	struct vfe_device *vfe = to_vfe(line);
    103
    104	vfe_disable_output(line);
    105
    106	vfe_put_output(line);
    107
    108	mutex_lock(&vfe->stream_lock);
    109
    110	if (vfe->stream_count == 1)
    111		vfe->ops_gen1->bus_enable_wr_if(vfe, 0);
    112
    113	vfe->stream_count--;
    114
    115	mutex_unlock(&vfe->stream_lock);
    116
    117	return 0;
    118}
    119
    120static void vfe_output_init_addrs(struct vfe_device *vfe,
    121				  struct vfe_output *output, u8 sync,
    122				  struct vfe_line *line)
    123{
    124	u32 ping_addr;
    125	u32 pong_addr;
    126	unsigned int i;
    127
    128	output->gen1.active_buf = 0;
    129
    130	for (i = 0; i < output->wm_num; i++) {
    131		if (output->buf[0])
    132			ping_addr = output->buf[0]->addr[i];
    133		else
    134			ping_addr = 0;
    135
    136		if (output->buf[1])
    137			pong_addr = output->buf[1]->addr[i];
    138		else
    139			pong_addr = ping_addr;
    140
    141		vfe->ops_gen1->wm_set_ping_addr(vfe, output->wm_idx[i], ping_addr);
    142		vfe->ops_gen1->wm_set_pong_addr(vfe, output->wm_idx[i], pong_addr);
    143		if (sync)
    144			vfe->ops_gen1->bus_reload_wm(vfe, output->wm_idx[i]);
    145	}
    146}
    147
    148static void vfe_output_frame_drop(struct vfe_device *vfe,
    149				  struct vfe_output *output,
    150				  u32 drop_pattern)
    151{
    152	u8 drop_period;
    153	unsigned int i;
    154
    155	/* We need to toggle update period to be valid on next frame */
    156	output->drop_update_idx++;
    157	output->drop_update_idx %= VFE_FRAME_DROP_UPDATES;
    158	drop_period = VFE_FRAME_DROP_VAL + output->drop_update_idx;
    159
    160	for (i = 0; i < output->wm_num; i++) {
    161		vfe->ops_gen1->wm_set_framedrop_period(vfe, output->wm_idx[i], drop_period);
    162		vfe->ops_gen1->wm_set_framedrop_pattern(vfe, output->wm_idx[i], drop_pattern);
    163	}
    164
    165	vfe->ops->reg_update(vfe, container_of(output, struct vfe_line, output)->id);
    166}
    167
    168static int vfe_enable_output(struct vfe_line *line)
    169{
    170	struct vfe_device *vfe = to_vfe(line);
    171	struct vfe_output *output = &line->output;
    172	const struct vfe_hw_ops *ops = vfe->ops;
    173	struct media_entity *sensor;
    174	unsigned long flags;
    175	unsigned int frame_skip = 0;
    176	unsigned int i;
    177	u16 ub_size;
    178
    179	ub_size = vfe->ops_gen1->get_ub_size(vfe->id);
    180	if (!ub_size)
    181		return -EINVAL;
    182
    183	sensor = camss_find_sensor(&line->subdev.entity);
    184	if (sensor) {
    185		struct v4l2_subdev *subdev = media_entity_to_v4l2_subdev(sensor);
    186
    187		v4l2_subdev_call(subdev, sensor, g_skip_frames, &frame_skip);
    188		/* Max frame skip is 29 frames */
    189		if (frame_skip > VFE_FRAME_DROP_VAL - 1)
    190			frame_skip = VFE_FRAME_DROP_VAL - 1;
    191	}
    192
    193	spin_lock_irqsave(&vfe->output_lock, flags);
    194
    195	ops->reg_update_clear(vfe, line->id);
    196
    197	if (output->state != VFE_OUTPUT_RESERVED) {
    198		dev_err(vfe->camss->dev, "Output is not in reserved state %d\n", output->state);
    199		spin_unlock_irqrestore(&vfe->output_lock, flags);
    200		return -EINVAL;
    201	}
    202	output->state = VFE_OUTPUT_IDLE;
    203
    204	output->buf[0] = vfe_buf_get_pending(output);
    205	output->buf[1] = vfe_buf_get_pending(output);
    206
    207	if (!output->buf[0] && output->buf[1]) {
    208		output->buf[0] = output->buf[1];
    209		output->buf[1] = NULL;
    210	}
    211
    212	if (output->buf[0])
    213		output->state = VFE_OUTPUT_SINGLE;
    214
    215	if (output->buf[1])
    216		output->state = VFE_OUTPUT_CONTINUOUS;
    217
    218	switch (output->state) {
    219	case VFE_OUTPUT_SINGLE:
    220		vfe_output_frame_drop(vfe, output, 1 << frame_skip);
    221		break;
    222	case VFE_OUTPUT_CONTINUOUS:
    223		vfe_output_frame_drop(vfe, output, 3 << frame_skip);
    224		break;
    225	default:
    226		vfe_output_frame_drop(vfe, output, 0);
    227		break;
    228	}
    229
    230	output->sequence = 0;
    231	output->gen1.wait_sof = 0;
    232	output->wait_reg_update = 0;
    233	reinit_completion(&output->sof);
    234	reinit_completion(&output->reg_update);
    235
    236	vfe_output_init_addrs(vfe, output, 0, line);
    237
    238	if (line->id != VFE_LINE_PIX) {
    239		vfe->ops_gen1->set_cgc_override(vfe, output->wm_idx[0], 1);
    240		vfe->ops_gen1->enable_irq_wm_line(vfe, output->wm_idx[0], line->id, 1);
    241		vfe->ops_gen1->bus_connect_wm_to_rdi(vfe, output->wm_idx[0], line->id);
    242		vfe->ops_gen1->wm_set_subsample(vfe, output->wm_idx[0]);
    243		vfe->ops_gen1->set_rdi_cid(vfe, line->id, 0);
    244		vfe->ops_gen1->wm_set_ub_cfg(vfe, output->wm_idx[0],
    245					    (ub_size + 1) * output->wm_idx[0], ub_size);
    246		vfe->ops_gen1->wm_frame_based(vfe, output->wm_idx[0], 1);
    247		vfe->ops_gen1->wm_enable(vfe, output->wm_idx[0], 1);
    248		vfe->ops_gen1->bus_reload_wm(vfe, output->wm_idx[0]);
    249	} else {
    250		ub_size /= output->wm_num;
    251		for (i = 0; i < output->wm_num; i++) {
    252			vfe->ops_gen1->set_cgc_override(vfe, output->wm_idx[i], 1);
    253			vfe->ops_gen1->wm_set_subsample(vfe, output->wm_idx[i]);
    254			vfe->ops_gen1->wm_set_ub_cfg(vfe, output->wm_idx[i],
    255						     (ub_size + 1) * output->wm_idx[i], ub_size);
    256			vfe->ops_gen1->wm_line_based(vfe, output->wm_idx[i],
    257						     &line->video_out.active_fmt.fmt.pix_mp, i, 1);
    258			vfe->ops_gen1->wm_enable(vfe, output->wm_idx[i], 1);
    259			vfe->ops_gen1->bus_reload_wm(vfe, output->wm_idx[i]);
    260		}
    261		vfe->ops_gen1->enable_irq_pix_line(vfe, 0, line->id, 1);
    262		vfe->ops_gen1->set_module_cfg(vfe, 1);
    263		vfe->ops_gen1->set_camif_cfg(vfe, line);
    264		vfe->ops_gen1->set_realign_cfg(vfe, line, 1);
    265		vfe->ops_gen1->set_xbar_cfg(vfe, output, 1);
    266		vfe->ops_gen1->set_demux_cfg(vfe, line);
    267		vfe->ops_gen1->set_scale_cfg(vfe, line);
    268		vfe->ops_gen1->set_crop_cfg(vfe, line);
    269		vfe->ops_gen1->set_clamp_cfg(vfe);
    270		vfe->ops_gen1->set_camif_cmd(vfe, 1);
    271	}
    272
    273	ops->reg_update(vfe, line->id);
    274
    275	spin_unlock_irqrestore(&vfe->output_lock, flags);
    276
    277	return 0;
    278}
    279
    280static int vfe_get_output(struct vfe_line *line)
    281{
    282	struct vfe_device *vfe = to_vfe(line);
    283	struct vfe_output *output;
    284	struct v4l2_format *f = &line->video_out.active_fmt;
    285	unsigned long flags;
    286	int i;
    287	int wm_idx;
    288
    289	spin_lock_irqsave(&vfe->output_lock, flags);
    290
    291	output = &line->output;
    292	if (output->state != VFE_OUTPUT_OFF) {
    293		dev_err(vfe->camss->dev, "Output is running\n");
    294		goto error;
    295	}
    296	output->state = VFE_OUTPUT_RESERVED;
    297
    298	output->gen1.active_buf = 0;
    299
    300	switch (f->fmt.pix_mp.pixelformat) {
    301	case V4L2_PIX_FMT_NV12:
    302	case V4L2_PIX_FMT_NV21:
    303	case V4L2_PIX_FMT_NV16:
    304	case V4L2_PIX_FMT_NV61:
    305		output->wm_num = 2;
    306		break;
    307	default:
    308		output->wm_num = 1;
    309		break;
    310	}
    311
    312	for (i = 0; i < output->wm_num; i++) {
    313		wm_idx = vfe_reserve_wm(vfe, line->id);
    314		if (wm_idx < 0) {
    315			dev_err(vfe->camss->dev, "Can not reserve wm\n");
    316			goto error_get_wm;
    317		}
    318		output->wm_idx[i] = wm_idx;
    319	}
    320
    321	output->drop_update_idx = 0;
    322
    323	spin_unlock_irqrestore(&vfe->output_lock, flags);
    324
    325	return 0;
    326
    327error_get_wm:
    328	for (i--; i >= 0; i--)
    329		vfe_release_wm(vfe, output->wm_idx[i]);
    330	output->state = VFE_OUTPUT_OFF;
    331error:
    332	spin_unlock_irqrestore(&vfe->output_lock, flags);
    333
    334	return -EINVAL;
    335}
    336
    337int vfe_gen1_enable(struct vfe_line *line)
    338{
    339	struct vfe_device *vfe = to_vfe(line);
    340	int ret;
    341
    342	mutex_lock(&vfe->stream_lock);
    343
    344	if (!vfe->stream_count) {
    345		vfe->ops_gen1->enable_irq_common(vfe);
    346		vfe->ops_gen1->bus_enable_wr_if(vfe, 1);
    347		vfe->ops_gen1->set_qos(vfe);
    348		vfe->ops_gen1->set_ds(vfe);
    349	}
    350
    351	vfe->stream_count++;
    352
    353	mutex_unlock(&vfe->stream_lock);
    354
    355	ret = vfe_get_output(line);
    356	if (ret < 0)
    357		goto error_get_output;
    358
    359	ret = vfe_enable_output(line);
    360	if (ret < 0)
    361		goto error_enable_output;
    362
    363	vfe->was_streaming = 1;
    364
    365	return 0;
    366
    367error_enable_output:
    368	vfe_put_output(line);
    369
    370error_get_output:
    371	mutex_lock(&vfe->stream_lock);
    372
    373	if (vfe->stream_count == 1)
    374		vfe->ops_gen1->bus_enable_wr_if(vfe, 0);
    375
    376	vfe->stream_count--;
    377
    378	mutex_unlock(&vfe->stream_lock);
    379
    380	return ret;
    381}
    382
    383static void vfe_output_update_ping_addr(struct vfe_device *vfe,
    384					struct vfe_output *output, u8 sync,
    385					struct vfe_line *line)
    386{
    387	u32 addr;
    388	unsigned int i;
    389
    390	for (i = 0; i < output->wm_num; i++) {
    391		if (output->buf[0])
    392			addr = output->buf[0]->addr[i];
    393		else
    394			addr = 0;
    395
    396		vfe->ops_gen1->wm_set_ping_addr(vfe, output->wm_idx[i], addr);
    397		if (sync)
    398			vfe->ops_gen1->bus_reload_wm(vfe, output->wm_idx[i]);
    399	}
    400}
    401
    402static void vfe_output_update_pong_addr(struct vfe_device *vfe,
    403					struct vfe_output *output, u8 sync,
    404					struct vfe_line *line)
    405{
    406	u32 addr;
    407	unsigned int i;
    408
    409	for (i = 0; i < output->wm_num; i++) {
    410		if (output->buf[1])
    411			addr = output->buf[1]->addr[i];
    412		else
    413			addr = 0;
    414
    415		vfe->ops_gen1->wm_set_pong_addr(vfe, output->wm_idx[i], addr);
    416		if (sync)
    417			vfe->ops_gen1->bus_reload_wm(vfe, output->wm_idx[i]);
    418	}
    419}
    420
    421static void vfe_buf_update_wm_on_next(struct vfe_device *vfe,
    422				      struct vfe_output *output)
    423{
    424	switch (output->state) {
    425	case VFE_OUTPUT_CONTINUOUS:
    426		vfe_output_frame_drop(vfe, output, 3);
    427		break;
    428	case VFE_OUTPUT_SINGLE:
    429	default:
    430		dev_err_ratelimited(vfe->camss->dev,
    431				    "Next buf in wrong state! %d\n",
    432				    output->state);
    433		break;
    434	}
    435}
    436
    437static void vfe_buf_update_wm_on_last(struct vfe_device *vfe,
    438				      struct vfe_output *output)
    439{
    440	switch (output->state) {
    441	case VFE_OUTPUT_CONTINUOUS:
    442		output->state = VFE_OUTPUT_SINGLE;
    443		vfe_output_frame_drop(vfe, output, 1);
    444		break;
    445	case VFE_OUTPUT_SINGLE:
    446		output->state = VFE_OUTPUT_STOPPING;
    447		vfe_output_frame_drop(vfe, output, 0);
    448		break;
    449	default:
    450		dev_err_ratelimited(vfe->camss->dev,
    451				    "Last buff in wrong state! %d\n",
    452				    output->state);
    453		break;
    454	}
    455}
    456
    457static void vfe_buf_update_wm_on_new(struct vfe_device *vfe,
    458				     struct vfe_output *output,
    459				     struct camss_buffer *new_buf,
    460				     struct vfe_line *line)
    461{
    462	int inactive_idx;
    463
    464	switch (output->state) {
    465	case VFE_OUTPUT_SINGLE:
    466		inactive_idx = !output->gen1.active_buf;
    467
    468		if (!output->buf[inactive_idx]) {
    469			output->buf[inactive_idx] = new_buf;
    470
    471			if (inactive_idx)
    472				vfe_output_update_pong_addr(vfe, output, 0, line);
    473			else
    474				vfe_output_update_ping_addr(vfe, output, 0, line);
    475
    476			vfe_output_frame_drop(vfe, output, 3);
    477			output->state = VFE_OUTPUT_CONTINUOUS;
    478		} else {
    479			vfe_buf_add_pending(output, new_buf);
    480			dev_err_ratelimited(vfe->camss->dev,
    481					    "Inactive buffer is busy\n");
    482		}
    483		break;
    484
    485	case VFE_OUTPUT_IDLE:
    486		if (!output->buf[0]) {
    487			output->buf[0] = new_buf;
    488
    489			vfe_output_init_addrs(vfe, output, 1, line);
    490			vfe_output_frame_drop(vfe, output, 1);
    491
    492			output->state = VFE_OUTPUT_SINGLE;
    493		} else {
    494			vfe_buf_add_pending(output, new_buf);
    495			dev_err_ratelimited(vfe->camss->dev,
    496					    "Output idle with buffer set!\n");
    497		}
    498		break;
    499
    500	case VFE_OUTPUT_CONTINUOUS:
    501	default:
    502		vfe_buf_add_pending(output, new_buf);
    503		break;
    504	}
    505}
    506
    507/*
    508 * vfe_isr_halt_ack - Process halt ack
    509 * @vfe: VFE Device
    510 */
    511static void vfe_isr_halt_ack(struct vfe_device *vfe)
    512{
    513	complete(&vfe->halt_complete);
    514	vfe->ops_gen1->halt_clear(vfe);
    515}
    516
    517/*
    518 * vfe_isr_sof - Process start of frame interrupt
    519 * @vfe: VFE Device
    520 * @line_id: VFE line
    521 */
    522static void vfe_isr_sof(struct vfe_device *vfe, enum vfe_line_id line_id)
    523{
    524	struct vfe_output *output;
    525	unsigned long flags;
    526
    527	spin_lock_irqsave(&vfe->output_lock, flags);
    528	output = &vfe->line[line_id].output;
    529	if (output->gen1.wait_sof) {
    530		output->gen1.wait_sof = 0;
    531		complete(&output->sof);
    532	}
    533	spin_unlock_irqrestore(&vfe->output_lock, flags);
    534}
    535
    536/*
    537 * vfe_isr_reg_update - Process reg update interrupt
    538 * @vfe: VFE Device
    539 * @line_id: VFE line
    540 */
    541static void vfe_isr_reg_update(struct vfe_device *vfe, enum vfe_line_id line_id)
    542{
    543	struct vfe_output *output;
    544	struct vfe_line *line = &vfe->line[line_id];
    545	unsigned long flags;
    546
    547	spin_lock_irqsave(&vfe->output_lock, flags);
    548	vfe->ops->reg_update_clear(vfe, line_id);
    549
    550	output = &line->output;
    551
    552	if (output->wait_reg_update) {
    553		output->wait_reg_update = 0;
    554		complete(&output->reg_update);
    555		spin_unlock_irqrestore(&vfe->output_lock, flags);
    556		return;
    557	}
    558
    559	if (output->state == VFE_OUTPUT_STOPPING) {
    560		/* Release last buffer when hw is idle */
    561		if (output->last_buffer) {
    562			vb2_buffer_done(&output->last_buffer->vb.vb2_buf,
    563					VB2_BUF_STATE_DONE);
    564			output->last_buffer = NULL;
    565		}
    566		output->state = VFE_OUTPUT_IDLE;
    567
    568		/* Buffers received in stopping state are queued in */
    569		/* dma pending queue, start next capture here */
    570
    571		output->buf[0] = vfe_buf_get_pending(output);
    572		output->buf[1] = vfe_buf_get_pending(output);
    573
    574		if (!output->buf[0] && output->buf[1]) {
    575			output->buf[0] = output->buf[1];
    576			output->buf[1] = NULL;
    577		}
    578
    579		if (output->buf[0])
    580			output->state = VFE_OUTPUT_SINGLE;
    581
    582		if (output->buf[1])
    583			output->state = VFE_OUTPUT_CONTINUOUS;
    584
    585		switch (output->state) {
    586		case VFE_OUTPUT_SINGLE:
    587			vfe_output_frame_drop(vfe, output, 2);
    588			break;
    589		case VFE_OUTPUT_CONTINUOUS:
    590			vfe_output_frame_drop(vfe, output, 3);
    591			break;
    592		default:
    593			vfe_output_frame_drop(vfe, output, 0);
    594			break;
    595		}
    596
    597		vfe_output_init_addrs(vfe, output, 1, &vfe->line[line_id]);
    598	}
    599
    600	spin_unlock_irqrestore(&vfe->output_lock, flags);
    601}
    602
    603/*
    604 * vfe_isr_wm_done - Process write master done interrupt
    605 * @vfe: VFE Device
    606 * @wm: Write master id
    607 */
    608static void vfe_isr_wm_done(struct vfe_device *vfe, u8 wm)
    609{
    610	struct camss_buffer *ready_buf;
    611	struct vfe_output *output;
    612	dma_addr_t *new_addr;
    613	unsigned long flags;
    614	u32 active_index;
    615	u64 ts = ktime_get_ns();
    616	unsigned int i;
    617
    618	active_index = vfe->ops_gen1->wm_get_ping_pong_status(vfe, wm);
    619
    620	spin_lock_irqsave(&vfe->output_lock, flags);
    621
    622	if (vfe->wm_output_map[wm] == VFE_LINE_NONE) {
    623		dev_err_ratelimited(vfe->camss->dev,
    624				    "Received wm done for unmapped index\n");
    625		goto out_unlock;
    626	}
    627	output = &vfe->line[vfe->wm_output_map[wm]].output;
    628
    629	if (output->gen1.active_buf == active_index && 0) {
    630		dev_err_ratelimited(vfe->camss->dev,
    631				    "Active buffer mismatch!\n");
    632		goto out_unlock;
    633	}
    634	output->gen1.active_buf = active_index;
    635
    636	ready_buf = output->buf[!active_index];
    637	if (!ready_buf) {
    638		dev_err_ratelimited(vfe->camss->dev,
    639				    "Missing ready buf %d %d!\n",
    640				    !active_index, output->state);
    641		goto out_unlock;
    642	}
    643
    644	ready_buf->vb.vb2_buf.timestamp = ts;
    645	ready_buf->vb.sequence = output->sequence++;
    646
    647	/* Get next buffer */
    648	output->buf[!active_index] = vfe_buf_get_pending(output);
    649	if (!output->buf[!active_index]) {
    650		/* No next buffer - set same address */
    651		new_addr = ready_buf->addr;
    652		vfe_buf_update_wm_on_last(vfe, output);
    653	} else {
    654		new_addr = output->buf[!active_index]->addr;
    655		vfe_buf_update_wm_on_next(vfe, output);
    656	}
    657
    658	if (active_index)
    659		for (i = 0; i < output->wm_num; i++)
    660			vfe->ops_gen1->wm_set_ping_addr(vfe, output->wm_idx[i], new_addr[i]);
    661	else
    662		for (i = 0; i < output->wm_num; i++)
    663			vfe->ops_gen1->wm_set_pong_addr(vfe, output->wm_idx[i], new_addr[i]);
    664
    665	spin_unlock_irqrestore(&vfe->output_lock, flags);
    666
    667	if (output->state == VFE_OUTPUT_STOPPING)
    668		output->last_buffer = ready_buf;
    669	else
    670		vb2_buffer_done(&ready_buf->vb.vb2_buf, VB2_BUF_STATE_DONE);
    671
    672	return;
    673
    674out_unlock:
    675	spin_unlock_irqrestore(&vfe->output_lock, flags);
    676}
    677
    678/*
    679 * vfe_queue_buffer - Add empty buffer
    680 * @vid: Video device structure
    681 * @buf: Buffer to be enqueued
    682 *
    683 * Add an empty buffer - depending on the current number of buffers it will be
    684 * put in pending buffer queue or directly given to the hardware to be filled.
    685 *
    686 * Return 0 on success or a negative error code otherwise
    687 */
    688static int vfe_queue_buffer(struct camss_video *vid, struct camss_buffer *buf)
    689{
    690	struct vfe_line *line = container_of(vid, struct vfe_line, video_out);
    691	struct vfe_device *vfe = to_vfe(line);
    692	struct vfe_output *output;
    693	unsigned long flags;
    694
    695	output = &line->output;
    696
    697	spin_lock_irqsave(&vfe->output_lock, flags);
    698
    699	vfe_buf_update_wm_on_new(vfe, output, buf, line);
    700
    701	spin_unlock_irqrestore(&vfe->output_lock, flags);
    702
    703	return 0;
    704}
    705
    706#define CALC_WORD(width, M, N) (((width) * (M) + (N) - 1) / (N))
    707
    708int vfe_word_per_line(u32 format, u32 width)
    709{
    710	int val = 0;
    711
    712	switch (format) {
    713	case V4L2_PIX_FMT_NV12:
    714	case V4L2_PIX_FMT_NV21:
    715	case V4L2_PIX_FMT_NV16:
    716	case V4L2_PIX_FMT_NV61:
    717		val = CALC_WORD(width, 1, 8);
    718		break;
    719	case V4L2_PIX_FMT_YUYV:
    720	case V4L2_PIX_FMT_YVYU:
    721	case V4L2_PIX_FMT_UYVY:
    722	case V4L2_PIX_FMT_VYUY:
    723		val = CALC_WORD(width, 2, 8);
    724		break;
    725	}
    726
    727	return val;
    728}
    729
    730const struct vfe_isr_ops vfe_isr_ops_gen1 = {
    731	.reset_ack = vfe_isr_reset_ack,
    732	.halt_ack = vfe_isr_halt_ack,
    733	.reg_update = vfe_isr_reg_update,
    734	.sof = vfe_isr_sof,
    735	.comp_done = vfe_isr_comp_done,
    736	.wm_done = vfe_isr_wm_done,
    737};
    738
    739const struct camss_video_ops vfe_video_ops_gen1 = {
    740	.queue_buffer = vfe_queue_buffer,
    741	.flush_buffers = vfe_flush_buffers,
    742};