atomisp_cmd.c

   1// SPDX-License-Identifier: GPL-2.0
   2/*
   3 * Support for Medifield PNW Camera Imaging ISP subsystem.
   4 *
   5 * Copyright (c) 2010 Intel Corporation. All Rights Reserved.
   6 *
   7 * Copyright (c) 2010 Silicon Hive www.siliconhive.com.
   8 */
   9#include <linux/errno.h>
  10#include <linux/firmware.h>
  11#include <linux/pci.h>
  12#include <linux/interrupt.h>
  13#include <linux/io.h>
  14#include <linux/kernel.h>
  15#include <linux/kfifo.h>
  16#include <linux/pm_runtime.h>
  17#include <linux/timer.h>
  18
  19#include <asm/iosf_mbi.h>
  20
  21#include <media/v4l2-event.h>
  22
  23#define CREATE_TRACE_POINTS
  24#include "atomisp_trace_event.h"
  25
  26#include "atomisp_cmd.h"
  27#include "atomisp_common.h"
  28#include "atomisp_fops.h"
  29#include "atomisp_internal.h"
  30#include "atomisp_ioctl.h"
  31#include "atomisp-regs.h"
  32#include "atomisp_tables.h"
  33#include "atomisp_compat.h"
  34#include "atomisp_subdev.h"
  35#include "atomisp_dfs_tables.h"
  36
  37#include <hmm/hmm.h>
  38
  39#include "sh_css_hrt.h"
  40#include "sh_css_defs.h"
  41#include "system_global.h"
  42#include "sh_css_internal.h"
  43#include "sh_css_sp.h"
  44#include "gp_device.h"
  45#include "device_access.h"
  46#include "irq.h"
  47
  48#include "ia_css_types.h"
  49#include "ia_css_stream.h"
  50#include "ia_css_debug.h"
  51#include "bits.h"
  52
  53union host {
  54	struct {
  55		void *kernel_ptr;
  56		void __user *user_ptr;
  57		int size;
  58	} scalar;
  59	struct {
  60		void *hmm_ptr;
  61	} ptr;
  62};
  63
  64/*
  65 * get sensor:dis71430/ov2720 related info from v4l2_subdev->priv data field.
  66 * subdev->priv is set in mrst.c
  67 */
  68struct camera_mipi_info *atomisp_to_sensor_mipi_info(struct v4l2_subdev *sd)
  69{
  70	return (struct camera_mipi_info *)v4l2_get_subdev_hostdata(sd);
  71}
  72
  73/*
  74 * get struct atomisp_video_pipe from v4l2 video_device
  75 */
  76struct atomisp_video_pipe *atomisp_to_video_pipe(struct video_device *dev)
  77{
  78	return (struct atomisp_video_pipe *)
  79	       container_of(dev, struct atomisp_video_pipe, vdev);
  80}
  81
  82static unsigned short atomisp_get_sensor_fps(struct atomisp_sub_device *asd)
  83{
  84	struct v4l2_subdev_frame_interval fi = { 0 };
  85	struct atomisp_device *isp = asd->isp;
  86
  87	unsigned short fps = 0;
  88	int ret;
  89
  90	ret = v4l2_subdev_call_state_active(isp->inputs[asd->input_curr].sensor,
  91					    pad, get_frame_interval, &fi);
  92
  93	if (!ret && fi.interval.numerator)
  94		fps = fi.interval.denominator / fi.interval.numerator;
  95
  96	return fps;
  97}
  98
  99/*
 100 * DFS progress is shown as follows:
 101 * 1. Target frequency is calculated according to FPS/Resolution/ISP running
 102 *    mode.
 103 * 2. Ratio is calculated using formula: 2 * HPLL / target frequency - 1
 104 *    with proper rounding.
 105 * 3. Set ratio to ISPFREQ40, 1 to FREQVALID and ISPFREQGUAR40
 106 *    to 200MHz in ISPSSPM1.
 107 * 4. Wait for FREQVALID to be cleared by P-Unit.
 108 * 5. Wait for field ISPFREQSTAT40 in ISPSSPM1 turn to ratio set in 3.
 109 */
 110static int write_target_freq_to_hw(struct atomisp_device *isp,
 111				   unsigned int new_freq)
 112{
 113	unsigned int ratio, timeout, guar_ratio;
 114	u32 isp_sspm1 = 0;
 115	int i;
 116
 117	if (!isp->hpll_freq) {
 118		dev_err(isp->dev, "failed to get hpll_freq. no change to freq\n");
 119		return -EINVAL;
 120	}
 121
 122	iosf_mbi_read(BT_MBI_UNIT_PMC, MBI_REG_READ, ISPSSPM1, &isp_sspm1);
 123	if (isp_sspm1 & ISP_FREQ_VALID_MASK) {
 124		dev_dbg(isp->dev, "clearing ISPSSPM1 valid bit.\n");
 125		iosf_mbi_write(BT_MBI_UNIT_PMC, MBI_REG_WRITE, ISPSSPM1,
 126			       isp_sspm1 & ~(1 << ISP_FREQ_VALID_OFFSET));
 127	}
 128
 129	ratio = (2 * isp->hpll_freq + new_freq / 2) / new_freq - 1;
 130	guar_ratio = (2 * isp->hpll_freq + 200 / 2) / 200 - 1;
 131
 132	iosf_mbi_read(BT_MBI_UNIT_PMC, MBI_REG_READ, ISPSSPM1, &isp_sspm1);
 133	isp_sspm1 &= ~(0x1F << ISP_REQ_FREQ_OFFSET);
 134
 135	for (i = 0; i < ISP_DFS_TRY_TIMES; i++) {
 136		iosf_mbi_write(BT_MBI_UNIT_PMC, MBI_REG_WRITE, ISPSSPM1,
 137			       isp_sspm1
 138			       | ratio << ISP_REQ_FREQ_OFFSET
 139			       | 1 << ISP_FREQ_VALID_OFFSET
 140			       | guar_ratio << ISP_REQ_GUAR_FREQ_OFFSET);
 141
 142		iosf_mbi_read(BT_MBI_UNIT_PMC, MBI_REG_READ, ISPSSPM1, &isp_sspm1);
 143		timeout = 20;
 144		while ((isp_sspm1 & ISP_FREQ_VALID_MASK) && timeout) {
 145			iosf_mbi_read(BT_MBI_UNIT_PMC, MBI_REG_READ, ISPSSPM1, &isp_sspm1);
 146			dev_dbg(isp->dev, "waiting for ISPSSPM1 valid bit to be 0.\n");
 147			udelay(100);
 148			timeout--;
 149		}
 150
 151		if (timeout != 0)
 152			break;
 153	}
 154
 155	if (timeout == 0) {
 156		dev_err(isp->dev, "DFS failed due to HW error.\n");
 157		return -EINVAL;
 158	}
 159
 160	iosf_mbi_read(BT_MBI_UNIT_PMC, MBI_REG_READ, ISPSSPM1, &isp_sspm1);
 161	timeout = 10;
 162	while (((isp_sspm1 >> ISP_FREQ_STAT_OFFSET) != ratio) && timeout) {
 163		iosf_mbi_read(BT_MBI_UNIT_PMC, MBI_REG_READ, ISPSSPM1, &isp_sspm1);
 164		dev_dbg(isp->dev, "waiting for ISPSSPM1 status bit to be 0x%x.\n",
 165			new_freq);
 166		udelay(100);
 167		timeout--;
 168	}
 169	if (timeout == 0) {
 170		dev_err(isp->dev, "DFS target freq is rejected by HW.\n");
 171		return -EINVAL;
 172	}
 173
 174	return 0;
 175}
 176
 177int atomisp_freq_scaling(struct atomisp_device *isp,
 178			 enum atomisp_dfs_mode mode,
 179			 bool force)
 180{
 181	const struct atomisp_dfs_config *dfs;
 182	unsigned int new_freq;
 183	struct atomisp_freq_scaling_rule curr_rules;
 184	int i, ret;
 185	unsigned short fps = 0;
 186
 187	dfs = isp->dfs;
 188
 189	if (dfs->lowest_freq == 0 || dfs->max_freq_at_vmin == 0 ||
 190	    dfs->highest_freq == 0 || dfs->dfs_table_size == 0 ||
 191	    !dfs->dfs_table) {
 192		dev_err(isp->dev, "DFS configuration is invalid.\n");
 193		return -EINVAL;
 194	}
 195
 196	if (mode == ATOMISP_DFS_MODE_LOW) {
 197		new_freq = dfs->lowest_freq;
 198		goto done;
 199	}
 200
 201	if (mode == ATOMISP_DFS_MODE_MAX) {
 202		new_freq = dfs->highest_freq;
 203		goto done;
 204	}
 205
 206	fps = atomisp_get_sensor_fps(&isp->asd);
 207	if (fps == 0) {
 208		dev_info(isp->dev,
 209			 "Sensor didn't report FPS. Using DFS max mode.\n");
 210		new_freq = dfs->highest_freq;
 211		goto done;
 212	}
 213
 214	curr_rules.width = isp->asd.fmt[ATOMISP_SUBDEV_PAD_SOURCE].fmt.width;
 215	curr_rules.height = isp->asd.fmt[ATOMISP_SUBDEV_PAD_SOURCE].fmt.height;
 216	curr_rules.fps = fps;
 217	curr_rules.run_mode = isp->asd.run_mode->val;
 218
 219	/* search for the target frequency by looping freq rules*/
 220	for (i = 0; i < dfs->dfs_table_size; i++) {
 221		if (curr_rules.width != dfs->dfs_table[i].width &&
 222		    dfs->dfs_table[i].width != ISP_FREQ_RULE_ANY)
 223			continue;
 224		if (curr_rules.height != dfs->dfs_table[i].height &&
 225		    dfs->dfs_table[i].height != ISP_FREQ_RULE_ANY)
 226			continue;
 227		if (curr_rules.fps != dfs->dfs_table[i].fps &&
 228		    dfs->dfs_table[i].fps != ISP_FREQ_RULE_ANY)
 229			continue;
 230		if (curr_rules.run_mode != dfs->dfs_table[i].run_mode &&
 231		    dfs->dfs_table[i].run_mode != ISP_FREQ_RULE_ANY)
 232			continue;
 233		break;
 234	}
 235
 236	if (i == dfs->dfs_table_size)
 237		new_freq = dfs->max_freq_at_vmin;
 238	else
 239		new_freq = dfs->dfs_table[i].isp_freq;
 240
 241done:
 242	dev_dbg(isp->dev, "DFS target frequency=%d.\n", new_freq);
 243
 244	if ((new_freq == isp->running_freq) && !force)
 245		return 0;
 246
 247	dev_dbg(isp->dev, "Programming DFS frequency to %d\n", new_freq);
 248
 249	ret = write_target_freq_to_hw(isp, new_freq);
 250	if (!ret) {
 251		isp->running_freq = new_freq;
 252		trace_ipu_pstate(new_freq, -1);
 253	}
 254	return ret;
 255}
 256
 257/*
 258 * reset and restore ISP
 259 */
 260int atomisp_reset(struct atomisp_device *isp)
 261{
 262	/* Reset ISP by power-cycling it */
 263	int ret = 0;
 264
 265	dev_dbg(isp->dev, "%s\n", __func__);
 266
 267	ret = atomisp_power_off(isp->dev);
 268	if (ret < 0)
 269		dev_err(isp->dev, "atomisp_power_off failed, %d\n", ret);
 270
 271	ret = atomisp_power_on(isp->dev);
 272	if (ret < 0) {
 273		dev_err(isp->dev, "atomisp_power_on failed, %d\n", ret);
 274		isp->isp_fatal_error = true;
 275	}
 276
 277	return ret;
 278}
 279
 280/*
 281 * interrupt disable functions
 282 */
 283static void disable_isp_irq(enum hrt_isp_css_irq irq)
 284{
 285	irq_disable_channel(IRQ0_ID, irq);
 286
 287	if (irq != hrt_isp_css_irq_sp)
 288		return;
 289
 290	cnd_sp_irq_enable(SP0_ID, false);
 291}
 292
 293/*
 294 * interrupt clean function
 295 */
 296static void clear_isp_irq(enum hrt_isp_css_irq irq)
 297{
 298	irq_clear_all(IRQ0_ID);
 299}
 300
 301void atomisp_msi_irq_init(struct atomisp_device *isp)
 302{
 303	struct pci_dev *pdev = to_pci_dev(isp->dev);
 304	u32 msg32;
 305	u16 msg16;
 306
 307	pci_read_config_dword(pdev, PCI_MSI_CAPID, &msg32);
 308	msg32 |= 1 << MSI_ENABLE_BIT;
 309	pci_write_config_dword(pdev, PCI_MSI_CAPID, msg32);
 310
 311	msg32 = (1 << INTR_IER) | (1 << INTR_IIR);
 312	pci_write_config_dword(pdev, PCI_INTERRUPT_CTRL, msg32);
 313
 314	pci_read_config_word(pdev, PCI_COMMAND, &msg16);
 315	msg16 |= (PCI_COMMAND_MEMORY | PCI_COMMAND_MASTER |
 316		  PCI_COMMAND_INTX_DISABLE);
 317	pci_write_config_word(pdev, PCI_COMMAND, msg16);
 318}
 319
 320void atomisp_msi_irq_uninit(struct atomisp_device *isp)
 321{
 322	struct pci_dev *pdev = to_pci_dev(isp->dev);
 323	u32 msg32;
 324	u16 msg16;
 325
 326	pci_read_config_dword(pdev, PCI_MSI_CAPID, &msg32);
 327	msg32 &=  ~(1 << MSI_ENABLE_BIT);
 328	pci_write_config_dword(pdev, PCI_MSI_CAPID, msg32);
 329
 330	msg32 = 0x0;
 331	pci_write_config_dword(pdev, PCI_INTERRUPT_CTRL, msg32);
 332
 333	pci_read_config_word(pdev, PCI_COMMAND, &msg16);
 334	msg16 &= ~(PCI_COMMAND_MASTER);
 335	pci_write_config_word(pdev, PCI_COMMAND, msg16);
 336}
 337
 338static void atomisp_sof_event(struct atomisp_sub_device *asd)
 339{
 340	struct v4l2_event event = {0};
 341
 342	event.type = V4L2_EVENT_FRAME_SYNC;
 343	event.u.frame_sync.frame_sequence = atomic_read(&asd->sof_count);
 344
 345	v4l2_event_queue(asd->subdev.devnode, &event);
 346}
 347
 348void atomisp_eof_event(struct atomisp_sub_device *asd, uint8_t exp_id)
 349{
 350	struct v4l2_event event = {0};
 351
 352	event.type = V4L2_EVENT_FRAME_END;
 353	event.u.frame_sync.frame_sequence = exp_id;
 354
 355	v4l2_event_queue(asd->subdev.devnode, &event);
 356}
 357
 358static void atomisp_3a_stats_ready_event(struct atomisp_sub_device *asd,
 359	uint8_t exp_id)
 360{
 361	struct v4l2_event event = {0};
 362
 363	event.type = V4L2_EVENT_ATOMISP_3A_STATS_READY;
 364	event.u.frame_sync.frame_sequence = exp_id;
 365
 366	v4l2_event_queue(asd->subdev.devnode, &event);
 367}
 368
 369static void atomisp_metadata_ready_event(struct atomisp_sub_device *asd,
 370	enum atomisp_metadata_type md_type)
 371{
 372	struct v4l2_event event = {0};
 373
 374	event.type = V4L2_EVENT_ATOMISP_METADATA_READY;
 375	event.u.data[0] = md_type;
 376
 377	v4l2_event_queue(asd->subdev.devnode, &event);
 378}
 379
 380static void atomisp_reset_event(struct atomisp_sub_device *asd)
 381{
 382	struct v4l2_event event = {0};
 383
 384	event.type = V4L2_EVENT_ATOMISP_CSS_RESET;
 385
 386	v4l2_event_queue(asd->subdev.devnode, &event);
 387}
 388
 389static void print_csi_rx_errors(enum mipi_port_id port,
 390				struct atomisp_device *isp)
 391{
 392	u32 infos = 0;
 393
 394	atomisp_css_rx_get_irq_info(port, &infos);
 395
 396	dev_err(isp->dev, "CSI Receiver port %d errors:\n", port);
 397	if (infos & IA_CSS_RX_IRQ_INFO_BUFFER_OVERRUN)
 398		dev_err(isp->dev, "  buffer overrun");
 399	if (infos & IA_CSS_RX_IRQ_INFO_ERR_SOT)
 400		dev_err(isp->dev, "  start-of-transmission error");
 401	if (infos & IA_CSS_RX_IRQ_INFO_ERR_SOT_SYNC)
 402		dev_err(isp->dev, "  start-of-transmission sync error");
 403	if (infos & IA_CSS_RX_IRQ_INFO_ERR_CONTROL)
 404		dev_err(isp->dev, "  control error");
 405	if (infos & IA_CSS_RX_IRQ_INFO_ERR_ECC_DOUBLE)
 406		dev_err(isp->dev, "  2 or more ECC errors");
 407	if (infos & IA_CSS_RX_IRQ_INFO_ERR_CRC)
 408		dev_err(isp->dev, "  CRC mismatch");
 409	if (infos & IA_CSS_RX_IRQ_INFO_ERR_UNKNOWN_ID)
 410		dev_err(isp->dev, "  unknown error");
 411	if (infos & IA_CSS_RX_IRQ_INFO_ERR_FRAME_SYNC)
 412		dev_err(isp->dev, "  frame sync error");
 413	if (infos & IA_CSS_RX_IRQ_INFO_ERR_FRAME_DATA)
 414		dev_err(isp->dev, "  frame data error");
 415	if (infos & IA_CSS_RX_IRQ_INFO_ERR_DATA_TIMEOUT)
 416		dev_err(isp->dev, "  data timeout");
 417	if (infos & IA_CSS_RX_IRQ_INFO_ERR_UNKNOWN_ESC)
 418		dev_err(isp->dev, "  unknown escape command entry");
 419	if (infos & IA_CSS_RX_IRQ_INFO_ERR_LINE_SYNC)
 420		dev_err(isp->dev, "  line sync error");
 421}
 422
 423/* Clear irq reg */
 424static void clear_irq_reg(struct atomisp_device *isp)
 425{
 426	struct pci_dev *pdev = to_pci_dev(isp->dev);
 427	u32 msg_ret;
 428
 429	pci_read_config_dword(pdev, PCI_INTERRUPT_CTRL, &msg_ret);
 430	msg_ret |= 1 << INTR_IIR;
 431	pci_write_config_dword(pdev, PCI_INTERRUPT_CTRL, msg_ret);
 432}
 433
 434/* interrupt handling function*/
 435irqreturn_t atomisp_isr(int irq, void *dev)
 436{
 437	struct atomisp_device *isp = (struct atomisp_device *)dev;
 438	struct atomisp_css_event eof_event;
 439	unsigned int irq_infos = 0;
 440	unsigned long flags;
 441	int err;
 442
 443	spin_lock_irqsave(&isp->lock, flags);
 444
 445	if (!isp->css_initialized) {
 446		spin_unlock_irqrestore(&isp->lock, flags);
 447		return IRQ_HANDLED;
 448	}
 449	err = atomisp_css_irq_translate(isp, &irq_infos);
 450	if (err) {
 451		spin_unlock_irqrestore(&isp->lock, flags);
 452		return IRQ_NONE;
 453	}
 454
 455	clear_irq_reg(isp);
 456
 457	if (!isp->asd.streaming)
 458		goto out_nowake;
 459
 460	if (irq_infos & IA_CSS_IRQ_INFO_CSS_RECEIVER_SOF) {
 461		atomic_inc(&isp->asd.sof_count);
 462		atomisp_sof_event(&isp->asd);
 463
 464		/*
 465		 * If sequence_temp and sequence are the same there where no frames
 466		 * lost so we can increase sequence_temp.
 467		 * If not then processing of frame is still in progress and driver
 468		 * needs to keep old sequence_temp value.
 469		 * NOTE: There is assumption here that ISP will not start processing
 470		 * next frame from sensor before old one is completely done.
 471		 */
 472		if (atomic_read(&isp->asd.sequence) == atomic_read(&isp->asd.sequence_temp))
 473			atomic_set(&isp->asd.sequence_temp, atomic_read(&isp->asd.sof_count));
 474
 475		dev_dbg_ratelimited(isp->dev, "irq:0x%x (SOF)\n", irq_infos);
 476		irq_infos &= ~IA_CSS_IRQ_INFO_CSS_RECEIVER_SOF;
 477	}
 478
 479	if (irq_infos & IA_CSS_IRQ_INFO_EVENTS_READY)
 480		atomic_set(&isp->asd.sequence, atomic_read(&isp->asd.sequence_temp));
 481
 482	if ((irq_infos & IA_CSS_IRQ_INFO_INPUT_SYSTEM_ERROR) ||
 483	    (irq_infos & IA_CSS_IRQ_INFO_IF_ERROR)) {
 484		/* handle mipi receiver error */
 485		u32 rx_infos;
 486		enum mipi_port_id port;
 487
 488		for (port = MIPI_PORT0_ID; port <= MIPI_PORT2_ID;
 489		     port++) {
 490			print_csi_rx_errors(port, isp);
 491			atomisp_css_rx_get_irq_info(port, &rx_infos);
 492			atomisp_css_rx_clear_irq_info(port, rx_infos);
 493		}
 494	}
 495
 496	if (irq_infos & IA_CSS_IRQ_INFO_ISYS_EVENTS_READY) {
 497		while (ia_css_dequeue_isys_event(&eof_event.event) == 0) {
 498			atomisp_eof_event(&isp->asd, eof_event.event.exp_id);
 499			dev_dbg_ratelimited(isp->dev, "ISYS event: EOF exp_id %d\n",
 500					    eof_event.event.exp_id);
 501		}
 502
 503		irq_infos &= ~IA_CSS_IRQ_INFO_ISYS_EVENTS_READY;
 504		if (irq_infos == 0)
 505			goto out_nowake;
 506	}
 507
 508	spin_unlock_irqrestore(&isp->lock, flags);
 509
 510	dev_dbg_ratelimited(isp->dev, "irq:0x%x (unhandled)\n", irq_infos);
 511
 512	return IRQ_WAKE_THREAD;
 513
 514out_nowake:
 515	spin_unlock_irqrestore(&isp->lock, flags);
 516
 517	if (irq_infos)
 518		dev_dbg_ratelimited(isp->dev, "irq:0x%x (ignored, as not streaming anymore)\n",
 519				    irq_infos);
 520
 521	return IRQ_HANDLED;
 522}
 523
 524void atomisp_clear_css_buffer_counters(struct atomisp_sub_device *asd)
 525{
 526	int i;
 527
 528	memset(asd->s3a_bufs_in_css, 0, sizeof(asd->s3a_bufs_in_css));
 529	for (i = 0; i < ATOMISP_INPUT_STREAM_NUM; i++)
 530		memset(asd->metadata_bufs_in_css[i], 0,
 531		       sizeof(asd->metadata_bufs_in_css[i]));
 532	asd->dis_bufs_in_css = 0;
 533}
 534
 535/* 0x100000 is the start of dmem inside SP */
 536#define SP_DMEM_BASE	0x100000
 537
 538void dump_sp_dmem(struct atomisp_device *isp, unsigned int addr,
 539		  unsigned int size)
 540{
 541	unsigned int data = 0;
 542	unsigned int size32 = DIV_ROUND_UP(size, sizeof(u32));
 543
 544	dev_dbg(isp->dev, "atomisp mmio base: %p\n", isp->base);
 545	dev_dbg(isp->dev, "%s, addr:0x%x, size: %d, size32: %d\n", __func__,
 546		addr, size, size32);
 547	if (size32 * 4 + addr > 0x4000) {
 548		dev_err(isp->dev, "illegal size (%d) or addr (0x%x)\n",
 549			size32, addr);
 550		return;
 551	}
 552	addr += SP_DMEM_BASE;
 553	addr &= 0x003FFFFF;
 554	do {
 555		data = readl(isp->base + addr);
 556		dev_dbg(isp->dev, "%s, \t [0x%x]:0x%x\n", __func__, addr, data);
 557		addr += sizeof(u32);
 558	} while (--size32);
 559}
 560
 561int atomisp_buffers_in_css(struct atomisp_video_pipe *pipe)
 562{
 563	unsigned long irqflags;
 564	struct list_head *pos;
 565	int buffers_in_css = 0;
 566
 567	spin_lock_irqsave(&pipe->irq_lock, irqflags);
 568
 569	list_for_each(pos, &pipe->buffers_in_css)
 570		buffers_in_css++;
 571
 572	spin_unlock_irqrestore(&pipe->irq_lock, irqflags);
 573
 574	return buffers_in_css;
 575}
 576
 577void atomisp_buffer_done(struct ia_css_frame *frame, enum vb2_buffer_state state)
 578{
 579	struct atomisp_video_pipe *pipe = vb_to_pipe(&frame->vb.vb2_buf);
 580
 581	lockdep_assert_held(&pipe->irq_lock);
 582
 583	frame->vb.vb2_buf.timestamp = ktime_get_ns();
 584	frame->vb.field = pipe->pix.field;
 585	frame->vb.sequence = atomic_read(&pipe->asd->sequence);
 586	list_del(&frame->queue);
 587	if (state == VB2_BUF_STATE_DONE)
 588		vb2_set_plane_payload(&frame->vb.vb2_buf, 0, pipe->pix.sizeimage);
 589	vb2_buffer_done(&frame->vb.vb2_buf, state);
 590}
 591
 592void atomisp_flush_video_pipe(struct atomisp_video_pipe *pipe, enum vb2_buffer_state state,
 593			      bool warn_on_css_frames)
 594{
 595	struct ia_css_frame *frame, *_frame;
 596	unsigned long irqflags;
 597
 598	spin_lock_irqsave(&pipe->irq_lock, irqflags);
 599
 600	list_for_each_entry_safe(frame, _frame, &pipe->buffers_in_css, queue) {
 601		if (warn_on_css_frames)
 602			dev_warn(pipe->isp->dev, "Warning: CSS frames queued on flush\n");
 603		atomisp_buffer_done(frame, state);
 604	}
 605
 606	list_for_each_entry_safe(frame, _frame, &pipe->activeq, queue)
 607		atomisp_buffer_done(frame, state);
 608
 609	list_for_each_entry_safe(frame, _frame, &pipe->buffers_waiting_for_param, queue) {
 610		pipe->frame_request_config_id[frame->vb.vb2_buf.index] = 0;
 611		atomisp_buffer_done(frame, state);
 612	}
 613
 614	spin_unlock_irqrestore(&pipe->irq_lock, irqflags);
 615}
 616
 617/* clean out the parameters that did not apply */
 618void atomisp_flush_params_queue(struct atomisp_video_pipe *pipe)
 619{
 620	struct atomisp_css_params_with_list *param;
 621
 622	while (!list_empty(&pipe->per_frame_params)) {
 623		param = list_entry(pipe->per_frame_params.next,
 624				   struct atomisp_css_params_with_list, list);
 625		list_del(&param->list);
 626		atomisp_free_css_parameters(&param->params);
 627		kvfree(param);
 628	}
 629}
 630
 631/* Re-queue per-frame parameters */
 632static void atomisp_recover_params_queue(struct atomisp_video_pipe *pipe)
 633{
 634	struct atomisp_css_params_with_list *param;
 635	int i;
 636
 637	for (i = 0; i < VIDEO_MAX_FRAME; i++) {
 638		param = pipe->frame_params[i];
 639		if (param)
 640			list_add_tail(&param->list, &pipe->per_frame_params);
 641		pipe->frame_params[i] = NULL;
 642	}
 643	atomisp_handle_parameter_and_buffer(pipe);
 644}
 645
 646void atomisp_buf_done(struct atomisp_sub_device *asd, int error,
 647		      enum ia_css_buffer_type buf_type,
 648		      enum ia_css_pipe_id css_pipe_id,
 649		      bool q_buffers, enum atomisp_input_stream_id stream_id)
 650{
 651	struct atomisp_video_pipe *pipe = NULL;
 652	struct atomisp_css_buffer buffer;
 653	bool requeue = false;
 654	unsigned long irqflags;
 655	struct ia_css_frame *frame = NULL;
 656	struct atomisp_s3a_buf *s3a_buf = NULL, *_s3a_buf_tmp, *s3a_iter;
 657	struct atomisp_dis_buf *dis_buf = NULL, *_dis_buf_tmp, *dis_iter;
 658	struct atomisp_metadata_buf *md_buf = NULL, *_md_buf_tmp, *md_iter;
 659	enum atomisp_metadata_type md_type;
 660	struct atomisp_device *isp = asd->isp;
 661	int i, err;
 662
 663	lockdep_assert_held(&isp->mutex);
 664
 665	if (
 666	    buf_type != IA_CSS_BUFFER_TYPE_METADATA &&
 667	    buf_type != IA_CSS_BUFFER_TYPE_3A_STATISTICS &&
 668	    buf_type != IA_CSS_BUFFER_TYPE_DIS_STATISTICS &&
 669	    buf_type != IA_CSS_BUFFER_TYPE_OUTPUT_FRAME &&
 670	    buf_type != IA_CSS_BUFFER_TYPE_SEC_OUTPUT_FRAME &&
 671	    buf_type != IA_CSS_BUFFER_TYPE_RAW_OUTPUT_FRAME &&
 672	    buf_type != IA_CSS_BUFFER_TYPE_SEC_VF_OUTPUT_FRAME &&
 673	    buf_type != IA_CSS_BUFFER_TYPE_VF_OUTPUT_FRAME) {
 674		dev_err(isp->dev, "%s, unsupported buffer type: %d\n",
 675			__func__, buf_type);
 676		return;
 677	}
 678
 679	memset(&buffer, 0, sizeof(struct atomisp_css_buffer));
 680	buffer.css_buffer.type = buf_type;
 681	err = atomisp_css_dequeue_buffer(asd, stream_id, css_pipe_id,
 682					 buf_type, &buffer);
 683	if (err) {
 684		dev_err(isp->dev,
 685			"atomisp_css_dequeue_buffer failed: 0x%x\n", err);
 686		return;
 687	}
 688
 689	switch (buf_type) {
 690	case IA_CSS_BUFFER_TYPE_3A_STATISTICS:
 691		list_for_each_entry_safe(s3a_iter, _s3a_buf_tmp,
 692					 &asd->s3a_stats_in_css, list) {
 693			if (s3a_iter->s3a_data ==
 694			    buffer.css_buffer.data.stats_3a) {
 695				list_del_init(&s3a_iter->list);
 696				list_add_tail(&s3a_iter->list,
 697					      &asd->s3a_stats_ready);
 698				s3a_buf = s3a_iter;
 699				break;
 700			}
 701		}
 702
 703		asd->s3a_bufs_in_css[css_pipe_id]--;
 704		atomisp_3a_stats_ready_event(asd, buffer.css_buffer.exp_id);
 705		if (s3a_buf)
 706			dev_dbg(isp->dev, "%s: s3a stat with exp_id %d is ready\n",
 707				__func__, s3a_buf->s3a_data->exp_id);
 708		else
 709			dev_dbg(isp->dev, "%s: s3a stat is ready with no exp_id found\n",
 710				__func__);
 711		break;
 712	case IA_CSS_BUFFER_TYPE_METADATA:
 713		if (error)
 714			break;
 715
 716		md_type = ATOMISP_MAIN_METADATA;
 717		list_for_each_entry_safe(md_iter, _md_buf_tmp,
 718					 &asd->metadata_in_css[md_type], list) {
 719			if (md_iter->metadata ==
 720			    buffer.css_buffer.data.metadata) {
 721				list_del_init(&md_iter->list);
 722				list_add_tail(&md_iter->list,
 723					      &asd->metadata_ready[md_type]);
 724				md_buf = md_iter;
 725				break;
 726			}
 727		}
 728		asd->metadata_bufs_in_css[stream_id][css_pipe_id]--;
 729		atomisp_metadata_ready_event(asd, md_type);
 730		if (md_buf)
 731			dev_dbg(isp->dev, "%s: metadata with exp_id %d is ready\n",
 732				__func__, md_buf->metadata->exp_id);
 733		else
 734			dev_dbg(isp->dev, "%s: metadata is ready with no exp_id found\n",
 735				__func__);
 736		break;
 737	case IA_CSS_BUFFER_TYPE_DIS_STATISTICS:
 738		list_for_each_entry_safe(dis_iter, _dis_buf_tmp,
 739					 &asd->dis_stats_in_css, list) {
 740			if (dis_iter->dis_data ==
 741			    buffer.css_buffer.data.stats_dvs) {
 742				spin_lock_irqsave(&asd->dis_stats_lock,
 743						  irqflags);
 744				list_del_init(&dis_iter->list);
 745				list_add(&dis_iter->list, &asd->dis_stats);
 746				asd->params.dis_proj_data_valid = true;
 747				spin_unlock_irqrestore(&asd->dis_stats_lock,
 748						       irqflags);
 749				dis_buf = dis_iter;
 750				break;
 751			}
 752		}
 753		asd->dis_bufs_in_css--;
 754		if (dis_buf)
 755			dev_dbg(isp->dev, "%s: dis stat with exp_id %d is ready\n",
 756				__func__, dis_buf->dis_data->exp_id);
 757		else
 758			dev_dbg(isp->dev, "%s: dis stat is ready with no exp_id found\n",
 759				__func__);
 760		break;
 761	case IA_CSS_BUFFER_TYPE_VF_OUTPUT_FRAME:
 762	case IA_CSS_BUFFER_TYPE_SEC_VF_OUTPUT_FRAME:
 763		frame = buffer.css_buffer.data.frame;
 764		if (!frame) {
 765			WARN_ON(1);
 766			break;
 767		}
 768		if (!frame->valid)
 769			error = true;
 770
 771		pipe = vb_to_pipe(&frame->vb.vb2_buf);
 772
 773		dev_dbg(isp->dev, "%s: vf frame with exp_id %d is ready\n",
 774			__func__, frame->exp_id);
 775		pipe->frame_config_id[frame->vb.vb2_buf.index] = frame->isp_config_id;
 776		break;
 777	case IA_CSS_BUFFER_TYPE_OUTPUT_FRAME:
 778	case IA_CSS_BUFFER_TYPE_SEC_OUTPUT_FRAME:
 779		frame = buffer.css_buffer.data.frame;
 780		if (!frame) {
 781			WARN_ON(1);
 782			break;
 783		}
 784
 785		if (!frame->valid)
 786			error = true;
 787
 788		pipe = vb_to_pipe(&frame->vb.vb2_buf);
 789
 790		dev_dbg(isp->dev, "%s: main frame with exp_id %d is ready\n",
 791			__func__, frame->exp_id);
 792
 793		i = frame->vb.vb2_buf.index;
 794
 795		/* free the parameters */
 796		if (pipe->frame_params[i]) {
 797			if (asd->params.dvs_6axis == pipe->frame_params[i]->params.dvs_6axis)
 798				asd->params.dvs_6axis = NULL;
 799			atomisp_free_css_parameters(&pipe->frame_params[i]->params);
 800			kvfree(pipe->frame_params[i]);
 801			pipe->frame_params[i] = NULL;
 802		}
 803
 804		pipe->frame_config_id[i] = frame->isp_config_id;
 805
 806		if (asd->params.css_update_params_needed) {
 807			atomisp_apply_css_parameters(asd,
 808						     &asd->params.css_param);
 809			if (asd->params.css_param.update_flag.dz_config)
 810				asd->params.config.dz_config = &asd->params.css_param.dz_config;
 811			/* New global dvs 6axis config should be blocked
 812			 * here if there's a buffer with per-frame parameters
 813			 * pending in CSS frame buffer queue.
 814			 * This is to aviod zooming vibration since global
 815			 * parameters take effect immediately while
 816			 * per-frame parameters are taken after previous
 817			 * buffers in CSS got processed.
 818			 */
 819			if (asd->params.dvs_6axis)
 820				atomisp_css_set_dvs_6axis(asd,
 821							  asd->params.dvs_6axis);
 822			else
 823				asd->params.css_update_params_needed = false;
 824			/* The update flag should not be cleaned here
 825			 * since it is still going to be used to make up
 826			 * following per-frame parameters.
 827			 * This will introduce more copy work since each
 828			 * time when updating global parameters, the whole
 829			 * parameter set are applied.
 830			 * FIXME: A new set of parameter copy functions can
 831			 * be added to make up per-frame parameters based on
 832			 * solid structures stored in asd->params.css_param
 833			 * instead of using shadow pointers in update flag.
 834			 */
 835			atomisp_css_update_isp_params(asd);
 836		}
 837		break;
 838	default:
 839		break;
 840	}
 841	if (frame) {
 842		spin_lock_irqsave(&pipe->irq_lock, irqflags);
 843		atomisp_buffer_done(frame, error ? VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE);
 844		spin_unlock_irqrestore(&pipe->irq_lock, irqflags);
 845	}
 846
 847	/*
 848	 * Requeue should only be done for 3a and dis buffers.
 849	 * Queue/dequeue order will change if driver recycles image buffers.
 850	 */
 851	if (requeue) {
 852		err = atomisp_css_queue_buffer(asd,
 853					       stream_id, css_pipe_id,
 854					       buf_type, &buffer);
 855		if (err)
 856			dev_err(isp->dev, "%s, q to css fails: %d\n",
 857				__func__, err);
 858		return;
 859	}
 860	if (!error && q_buffers)
 861		atomisp_qbuffers_to_css(asd);
 862}
 863
 864void atomisp_assert_recovery_work(struct work_struct *work)
 865{
 866	struct atomisp_device *isp = container_of(work, struct atomisp_device,
 867						  assert_recovery_work);
 868	struct pci_dev *pdev = to_pci_dev(isp->dev);
 869	unsigned long flags;
 870	int ret;
 871
 872	mutex_lock(&isp->mutex);
 873
 874	if (!isp->asd.streaming)
 875		goto out_unlock;
 876
 877	atomisp_css_irq_enable(isp, IA_CSS_IRQ_INFO_CSS_RECEIVER_SOF, false);
 878
 879	spin_lock_irqsave(&isp->lock, flags);
 880	isp->asd.streaming = false;
 881	spin_unlock_irqrestore(&isp->lock, flags);
 882
 883	/* stream off sensor */
 884	ret = v4l2_subdev_call(isp->inputs[isp->asd.input_curr].csi_remote_source,
 885			       video, s_stream, 0);
 886	if (ret)
 887		dev_warn(isp->dev, "Stopping sensor stream failed: %d\n", ret);
 888
 889	atomisp_clear_css_buffer_counters(&isp->asd);
 890
 891	atomisp_css_stop(&isp->asd, true);
 892
 893	isp->asd.preview_exp_id = 1;
 894	isp->asd.postview_exp_id = 1;
 895	/* notify HAL the CSS reset */
 896	dev_dbg(isp->dev, "send reset event to %s\n", isp->asd.subdev.devnode->name);
 897	atomisp_reset_event(&isp->asd);
 898
 899	/* clear irq */
 900	disable_isp_irq(hrt_isp_css_irq_sp);
 901	clear_isp_irq(hrt_isp_css_irq_sp);
 902
 903	/* Set the SRSE to 3 before resetting */
 904	pci_write_config_dword(pdev, PCI_I_CONTROL,
 905			       isp->saved_regs.i_control | MRFLD_PCI_I_CONTROL_SRSE_RESET_MASK);
 906
 907	/* reset ISP and restore its state */
 908	atomisp_reset(isp);
 909
 910	atomisp_css_input_set_mode(&isp->asd, IA_CSS_INPUT_MODE_BUFFERED_SENSOR);
 911
 912	/* Recreate streams destroyed by atomisp_css_stop() */
 913	atomisp_create_pipes_stream(&isp->asd);
 914
 915	/* Invalidate caches. FIXME: should flush only necessary buffers */
 916	wbinvd();
 917
 918	if (atomisp_css_start(&isp->asd)) {
 919		dev_warn(isp->dev, "start SP failed, so do not set streaming to be enable!\n");
 920	} else {
 921		spin_lock_irqsave(&isp->lock, flags);
 922		isp->asd.streaming = true;
 923		spin_unlock_irqrestore(&isp->lock, flags);
 924	}
 925
 926	atomisp_csi2_configure(&isp->asd);
 927
 928	atomisp_css_irq_enable(isp, IA_CSS_IRQ_INFO_CSS_RECEIVER_SOF,
 929			       atomisp_css_valid_sof(isp));
 930
 931	if (atomisp_freq_scaling(isp, ATOMISP_DFS_MODE_AUTO, true) < 0)
 932		dev_dbg(isp->dev, "DFS auto failed while recovering!\n");
 933
 934	/* Dequeueing buffers is not needed, CSS will recycle buffers that it has */
 935	atomisp_flush_video_pipe(&isp->asd.video_out, VB2_BUF_STATE_ERROR, false);
 936
 937	/* Requeue unprocessed per-frame parameters. */
 938	atomisp_recover_params_queue(&isp->asd.video_out);
 939
 940	ret = v4l2_subdev_call(isp->inputs[isp->asd.input_curr].csi_remote_source,
 941			       video, s_stream, 1);
 942	if (ret)
 943		dev_err(isp->dev, "Starting sensor stream failed: %d\n", ret);
 944
 945out_unlock:
 946	mutex_unlock(&isp->mutex);
 947}
 948
 949irqreturn_t atomisp_isr_thread(int irq, void *isp_ptr)
 950{
 951	struct atomisp_device *isp = isp_ptr;
 952	unsigned long flags;
 953	bool streaming;
 954
 955	spin_lock_irqsave(&isp->lock, flags);
 956	streaming = isp->asd.streaming;
 957	spin_unlock_irqrestore(&isp->lock, flags);
 958
 959	if (!streaming)
 960		return IRQ_HANDLED;
 961
 962	/*
 963	 * The standard CSS2.0 API tells the following calling sequence of
 964	 * dequeue ready buffers:
 965	 * while (ia_css_dequeue_psys_event(...)) {
 966	 *	switch (event.type) {
 967	 *	...
 968	 *	ia_css_pipe_dequeue_buffer()
 969	 *	}
 970	 * }
 971	 * That is, dequeue event and buffer are one after another.
 972	 *
 973	 * But the following implementation is to first deuque all the event
 974	 * to a FIFO, then process the event in the FIFO.
 975	 * This will not have issue in single stream mode, but it do have some
 976	 * issue in multiple stream case. The issue is that
 977	 * ia_css_pipe_dequeue_buffer() will not return the corrent buffer in
 978	 * a specific pipe.
 979	 *
 980	 * This is due to ia_css_pipe_dequeue_buffer() does not take the
 981	 * ia_css_pipe parameter.
 982	 *
 983	 * So:
 984	 * For CSS2.0: we change the way to not dequeue all the event at one
 985	 * time, instead, dequue one and process one, then another
 986	 */
 987	mutex_lock(&isp->mutex);
 988	atomisp_css_isr_thread(isp);
 989	mutex_unlock(&isp->mutex);
 990
 991	return IRQ_HANDLED;
 992}
 993
 994/*
 995 * Get internal fmt according to V4L2 fmt
 996 */
 997static enum ia_css_frame_format
 998v4l2_fmt_to_sh_fmt(u32 fmt)
 999{
1000	switch (fmt) {
1001	case V4L2_PIX_FMT_YUV420:
1002				return IA_CSS_FRAME_FORMAT_YUV420;
1003	case V4L2_PIX_FMT_YVU420:
1004		return IA_CSS_FRAME_FORMAT_YV12;
1005	case V4L2_PIX_FMT_YUV422P:
1006		return IA_CSS_FRAME_FORMAT_YUV422;
1007	case V4L2_PIX_FMT_YUV444:
1008		return IA_CSS_FRAME_FORMAT_YUV444;
1009	case V4L2_PIX_FMT_NV12:
1010		return IA_CSS_FRAME_FORMAT_NV12;
1011	case V4L2_PIX_FMT_NV21:
1012		return IA_CSS_FRAME_FORMAT_NV21;
1013	case V4L2_PIX_FMT_NV16:
1014		return IA_CSS_FRAME_FORMAT_NV16;
1015	case V4L2_PIX_FMT_NV61:
1016		return IA_CSS_FRAME_FORMAT_NV61;
1017	case V4L2_PIX_FMT_UYVY:
1018		return IA_CSS_FRAME_FORMAT_UYVY;
1019	case V4L2_PIX_FMT_YUYV:
1020		return IA_CSS_FRAME_FORMAT_YUYV;
1021	case V4L2_PIX_FMT_RGB24:
1022		return IA_CSS_FRAME_FORMAT_PLANAR_RGB888;
1023	case V4L2_PIX_FMT_RGBX32:
1024		return IA_CSS_FRAME_FORMAT_RGBA888;
1025	case V4L2_PIX_FMT_RGB565:
1026		return IA_CSS_FRAME_FORMAT_RGB565;
1027#if 0
1028	case V4L2_PIX_FMT_JPEG:
1029	case V4L2_PIX_FMT_CUSTOM_M10MO_RAW:
1030		return IA_CSS_FRAME_FORMAT_BINARY_8;
1031#endif
1032	case V4L2_PIX_FMT_SBGGR16:
1033	case V4L2_PIX_FMT_SBGGR10:
1034	case V4L2_PIX_FMT_SGBRG10:
1035	case V4L2_PIX_FMT_SGRBG10:
1036	case V4L2_PIX_FMT_SRGGB10:
1037	case V4L2_PIX_FMT_SBGGR12:
1038	case V4L2_PIX_FMT_SGBRG12:
1039	case V4L2_PIX_FMT_SGRBG12:
1040	case V4L2_PIX_FMT_SRGGB12:
1041	case V4L2_PIX_FMT_SBGGR8:
1042	case V4L2_PIX_FMT_SGBRG8:
1043	case V4L2_PIX_FMT_SGRBG8:
1044	case V4L2_PIX_FMT_SRGGB8:
1045		return IA_CSS_FRAME_FORMAT_RAW;
1046	default:
1047		return -EINVAL;
1048	}
1049}
1050
1051/*
1052 * raw format match between SH format and V4L2 format
1053 */
1054static int raw_output_format_match_input(u32 input, u32 output)
1055{
1056	if ((input == ATOMISP_INPUT_FORMAT_RAW_12) &&
1057	    ((output == V4L2_PIX_FMT_SRGGB12) ||
1058	     (output == V4L2_PIX_FMT_SGRBG12) ||
1059	     (output == V4L2_PIX_FMT_SBGGR12) ||
1060	     (output == V4L2_PIX_FMT_SGBRG12)))
1061		return 0;
1062
1063	if ((input == ATOMISP_INPUT_FORMAT_RAW_10) &&
1064	    ((output == V4L2_PIX_FMT_SRGGB10) ||
1065	     (output == V4L2_PIX_FMT_SGRBG10) ||
1066	     (output == V4L2_PIX_FMT_SBGGR10) ||
1067	     (output == V4L2_PIX_FMT_SGBRG10)))
1068		return 0;
1069
1070	if ((input == ATOMISP_INPUT_FORMAT_RAW_8) &&
1071	    ((output == V4L2_PIX_FMT_SRGGB8) ||
1072	     (output == V4L2_PIX_FMT_SGRBG8) ||
1073	     (output == V4L2_PIX_FMT_SBGGR8) ||
1074	     (output == V4L2_PIX_FMT_SGBRG8)))
1075		return 0;
1076
1077	if ((input == ATOMISP_INPUT_FORMAT_RAW_16) && (output == V4L2_PIX_FMT_SBGGR16))
1078		return 0;
1079
1080	return -EINVAL;
1081}
1082
1083u32 atomisp_get_pixel_depth(u32 pixelformat)
1084{
1085	switch (pixelformat) {
1086	case V4L2_PIX_FMT_YUV420:
1087	case V4L2_PIX_FMT_NV12:
1088	case V4L2_PIX_FMT_NV21:
1089	case V4L2_PIX_FMT_YVU420:
1090		return 12;
1091	case V4L2_PIX_FMT_YUV422P:
1092	case V4L2_PIX_FMT_YUYV:
1093	case V4L2_PIX_FMT_UYVY:
1094	case V4L2_PIX_FMT_NV16:
1095	case V4L2_PIX_FMT_NV61:
1096	case V4L2_PIX_FMT_RGB565:
1097	case V4L2_PIX_FMT_SBGGR16:
1098	case V4L2_PIX_FMT_SBGGR12:
1099	case V4L2_PIX_FMT_SGBRG12:
1100	case V4L2_PIX_FMT_SGRBG12:
1101	case V4L2_PIX_FMT_SRGGB12:
1102	case V4L2_PIX_FMT_SBGGR10:
1103	case V4L2_PIX_FMT_SGBRG10:
1104	case V4L2_PIX_FMT_SGRBG10:
1105	case V4L2_PIX_FMT_SRGGB10:
1106		return 16;
1107	case V4L2_PIX_FMT_RGB24:
1108	case V4L2_PIX_FMT_YUV444:
1109		return 24;
1110	case V4L2_PIX_FMT_RGBX32:
1111		return 32;
1112	case V4L2_PIX_FMT_JPEG:
1113	case V4L2_PIX_FMT_CUSTOM_M10MO_RAW:
1114	case V4L2_PIX_FMT_SBGGR8:
1115	case V4L2_PIX_FMT_SGBRG8:
1116	case V4L2_PIX_FMT_SGRBG8:
1117	case V4L2_PIX_FMT_SRGGB8:
1118		return 8;
1119	default:
1120		return 8 * 2;	/* raw type now */
1121	}
1122}
1123
1124bool atomisp_is_mbuscode_raw(uint32_t code)
1125{
1126	return code >= 0x3000 && code < 0x4000;
1127}
1128
1129/*
1130 * ISP features control function
1131 */
1132
1133/*
1134 * Set ISP capture mode based on current settings
1135 */
1136static void atomisp_update_capture_mode(struct atomisp_sub_device *asd)
1137{
1138	if (asd->params.gdc_cac_en)
1139		atomisp_css_capture_set_mode(asd, IA_CSS_CAPTURE_MODE_ADVANCED);
1140	else if (asd->params.low_light)
1141		atomisp_css_capture_set_mode(asd, IA_CSS_CAPTURE_MODE_LOW_LIGHT);
1142	else if (asd->video_out.sh_fmt == IA_CSS_FRAME_FORMAT_RAW)
1143		atomisp_css_capture_set_mode(asd, IA_CSS_CAPTURE_MODE_RAW);
1144	else
1145		atomisp_css_capture_set_mode(asd, IA_CSS_CAPTURE_MODE_PRIMARY);
1146}
1147
1148/*
1149 * Function to enable/disable lens geometry distortion correction (GDC) and
1150 * chromatic aberration correction (CAC)
1151 */
1152int atomisp_gdc_cac(struct atomisp_sub_device *asd, int flag,
1153		    __s32 *value)
1154{
1155	if (flag == 0) {
1156		*value = asd->params.gdc_cac_en;
1157		return 0;
1158	}
1159
1160	asd->params.gdc_cac_en = !!*value;
1161	if (asd->params.gdc_cac_en)
1162		asd->params.config.morph_table = asd->params.css_param.morph_table;
1163	else
1164		asd->params.config.morph_table = NULL;
1165
1166	asd->params.css_update_params_needed = true;
1167	atomisp_update_capture_mode(asd);
1168	return 0;
1169}
1170
1171/*
1172 * Function to enable/disable low light mode including ANR
1173 */
1174int atomisp_low_light(struct atomisp_sub_device *asd, int flag,
1175		      __s32 *value)
1176{
1177	if (flag == 0) {
1178		*value = asd->params.low_light;
1179		return 0;
1180	}
1181
1182	asd->params.low_light = (*value != 0);
1183	atomisp_update_capture_mode(asd);
1184	return 0;
1185}
1186
1187/*
1188 * Function to enable/disable extra noise reduction (XNR) in low light
1189 * condition
1190 */
1191int atomisp_xnr(struct atomisp_sub_device *asd, int flag,
1192		int *xnr_enable)
1193{
1194	if (flag == 0) {
1195		*xnr_enable = asd->params.xnr_en;
1196		return 0;
1197	}
1198
1199	atomisp_css_capture_enable_xnr(asd, !!*xnr_enable);
1200
1201	return 0;
1202}
1203
1204/*
1205 * Function to configure bayer noise reduction
1206 */
1207int atomisp_nr(struct atomisp_sub_device *asd, int flag,
1208	       struct atomisp_nr_config *arg)
1209{
1210	if (flag == 0) {
1211		/* Get nr config from current setup */
1212		if (atomisp_css_get_nr_config(asd, arg))
1213			return -EINVAL;
1214	} else {
1215		/* Set nr config to isp parameters */
1216		memcpy(&asd->params.css_param.nr_config, arg,
1217		       sizeof(struct ia_css_nr_config));
1218		asd->params.config.nr_config = &asd->params.css_param.nr_config;
1219		asd->params.css_update_params_needed = true;
1220	}
1221	return 0;
1222}
1223
1224/*
1225 * Function to configure temporal noise reduction (TNR)
1226 */
1227int atomisp_tnr(struct atomisp_sub_device *asd, int flag,
1228		struct atomisp_tnr_config *config)
1229{
1230	/* Get tnr config from current setup */
1231	if (flag == 0) {
1232		/* Get tnr config from current setup */
1233		if (atomisp_css_get_tnr_config(asd, config))
1234			return -EINVAL;
1235	} else {
1236		/* Set tnr config to isp parameters */
1237		memcpy(&asd->params.css_param.tnr_config, config,
1238		       sizeof(struct ia_css_tnr_config));
1239		asd->params.config.tnr_config = &asd->params.css_param.tnr_config;
1240		asd->params.css_update_params_needed = true;
1241	}
1242
1243	return 0;
1244}
1245
1246/*
1247 * Function to configure black level compensation
1248 */
1249int atomisp_black_level(struct atomisp_sub_device *asd, int flag,
1250			struct atomisp_ob_config *config)
1251{
1252	if (flag == 0) {
1253		/* Get ob config from current setup */
1254		if (atomisp_css_get_ob_config(asd, config))
1255			return -EINVAL;
1256	} else {
1257		/* Set ob config to isp parameters */
1258		memcpy(&asd->params.css_param.ob_config, config,
1259		       sizeof(struct ia_css_ob_config));
1260		asd->params.config.ob_config = &asd->params.css_param.ob_config;
1261		asd->params.css_update_params_needed = true;
1262	}
1263
1264	return 0;
1265}
1266
1267/*
1268 * Function to configure edge enhancement
1269 */
1270int atomisp_ee(struct atomisp_sub_device *asd, int flag,
1271	       struct atomisp_ee_config *config)
1272{
1273	if (flag == 0) {
1274		/* Get ee config from current setup */
1275		if (atomisp_css_get_ee_config(asd, config))
1276			return -EINVAL;
1277	} else {
1278		/* Set ee config to isp parameters */
1279		memcpy(&asd->params.css_param.ee_config, config,
1280		       sizeof(asd->params.css_param.ee_config));
1281		asd->params.config.ee_config = &asd->params.css_param.ee_config;
1282		asd->params.css_update_params_needed = true;
1283	}
1284
1285	return 0;
1286}
1287
1288/*
1289 * Function to update Gamma table for gamma, brightness and contrast config
1290 */
1291int atomisp_gamma(struct atomisp_sub_device *asd, int flag,
1292		  struct atomisp_gamma_table *config)
1293{
1294	if (flag == 0) {
1295		/* Get gamma table from current setup */
1296		if (atomisp_css_get_gamma_table(asd, config))
1297			return -EINVAL;
1298	} else {
1299		/* Set gamma table to isp parameters */
1300		memcpy(&asd->params.css_param.gamma_table, config,
1301		       sizeof(asd->params.css_param.gamma_table));
1302		asd->params.config.gamma_table = &asd->params.css_param.gamma_table;
1303	}
1304
1305	return 0;
1306}
1307
1308/*
1309 * Function to update Ctc table for Chroma Enhancement
1310 */
1311int atomisp_ctc(struct atomisp_sub_device *asd, int flag,
1312		struct atomisp_ctc_table *config)
1313{
1314	if (flag == 0) {
1315		/* Get ctc table from current setup */
1316		if (atomisp_css_get_ctc_table(asd, config))
1317			return -EINVAL;
1318	} else {
1319		/* Set ctc table to isp parameters */
1320		memcpy(&asd->params.css_param.ctc_table, config,
1321		       sizeof(asd->params.css_param.ctc_table));
1322		atomisp_css_set_ctc_table(asd, &asd->params.css_param.ctc_table);
1323	}
1324
1325	return 0;
1326}
1327
1328/*
1329 * Function to update gamma correction parameters
1330 */
1331int atomisp_gamma_correction(struct atomisp_sub_device *asd, int flag,
1332			     struct atomisp_gc_config *config)
1333{
1334	if (flag == 0) {
1335		/* Get gamma correction params from current setup */
1336		if (atomisp_css_get_gc_config(asd, config))
1337			return -EINVAL;
1338	} else {
1339		/* Set gamma correction params to isp parameters */
1340		memcpy(&asd->params.css_param.gc_config, config,
1341		       sizeof(asd->params.css_param.gc_config));
1342		asd->params.config.gc_config = &asd->params.css_param.gc_config;
1343		asd->params.css_update_params_needed = true;
1344	}
1345
1346	return 0;
1347}
1348
1349/*
1350 * Function to update narrow gamma flag
1351 */
1352int atomisp_formats(struct atomisp_sub_device *asd, int flag,
1353		    struct atomisp_formats_config *config)
1354{
1355	if (flag == 0) {
1356		/* Get narrow gamma flag from current setup */
1357		if (atomisp_css_get_formats_config(asd, config))
1358			return -EINVAL;
1359	} else {
1360		/* Set narrow gamma flag to isp parameters */
1361		memcpy(&asd->params.css_param.formats_config, config,
1362		       sizeof(asd->params.css_param.formats_config));
1363		asd->params.config.formats_config = &asd->params.css_param.formats_config;
1364	}
1365
1366	return 0;
1367}
1368
1369void atomisp_free_internal_buffers(struct atomisp_sub_device *asd)
1370{
1371	atomisp_free_css_parameters(&asd->params.css_param);
1372}
1373
1374static void atomisp_update_grid_info(struct atomisp_sub_device *asd,
1375				     enum ia_css_pipe_id pipe_id)
1376{
1377	struct atomisp_device *isp = asd->isp;
1378	int err;
1379
1380	if (atomisp_css_get_grid_info(asd, pipe_id))
1381		return;
1382
1383	/* We must free all buffers because they no longer match
1384	   the grid size. */
1385	atomisp_css_free_stat_buffers(asd);
1386
1387	err = atomisp_alloc_css_stat_bufs(asd, ATOMISP_INPUT_STREAM_GENERAL);
1388	if (err) {
1389		dev_err(isp->dev, "stat_buf allocate error\n");
1390		goto err;
1391	}
1392
1393	if (atomisp_alloc_3a_output_buf(asd)) {
1394		/* Failure for 3A buffers does not influence DIS buffers */
1395		if (asd->params.s3a_output_bytes != 0) {
1396			/* For SOC sensor happens s3a_output_bytes == 0,
1397			 * using if condition to exclude false error log */
1398			dev_err(isp->dev, "Failed to allocate memory for 3A statistics\n");
1399		}
1400		goto err;
1401	}
1402
1403	if (atomisp_alloc_dis_coef_buf(asd)) {
1404		dev_err(isp->dev,
1405			"Failed to allocate memory for DIS statistics\n");
1406		goto err;
1407	}
1408
1409	if (atomisp_alloc_metadata_output_buf(asd)) {
1410		dev_err(isp->dev, "Failed to allocate memory for metadata\n");
1411		goto err;
1412	}
1413
1414	return;
1415
1416err:
1417	atomisp_css_free_stat_buffers(asd);
1418	return;
1419}
1420
1421static void atomisp_curr_user_grid_info(struct atomisp_sub_device *asd,
1422					struct atomisp_grid_info *info)
1423{
1424	memcpy(info, &asd->params.curr_grid_info.s3a_grid,
1425	       sizeof(struct ia_css_3a_grid_info));
1426}
1427
1428int atomisp_compare_grid(struct atomisp_sub_device *asd,
1429			 struct atomisp_grid_info *atomgrid)
1430{
1431	struct atomisp_grid_info tmp = {0};
1432
1433	atomisp_curr_user_grid_info(asd, &tmp);
1434	return memcmp(atomgrid, &tmp, sizeof(tmp));
1435}
1436
1437/*
1438 * Function to update Gdc table for gdc
1439 */
1440int atomisp_gdc_cac_table(struct atomisp_sub_device *asd, int flag,
1441			  struct atomisp_morph_table *config)
1442{
1443	int ret;
1444	int i;
1445	struct atomisp_device *isp = asd->isp;
1446
1447	if (flag == 0) {
1448		/* Get gdc table from current setup */
1449		struct ia_css_morph_table tab = {0};
1450
1451		atomisp_css_get_morph_table(asd, &tab);
1452
1453		config->width = tab.width;
1454		config->height = tab.height;
1455
1456		for (i = 0; i < IA_CSS_MORPH_TABLE_NUM_PLANES; i++) {
1457			ret = copy_to_user(config->coordinates_x[i],
1458					   tab.coordinates_x[i], tab.height *
1459					   tab.width * sizeof(*tab.coordinates_x[i]));
1460			if (ret) {
1461				dev_err(isp->dev,
1462					"Failed to copy to User for x\n");
1463				return -EFAULT;
1464			}
1465			ret = copy_to_user(config->coordinates_y[i],
1466					   tab.coordinates_y[i], tab.height *
1467					   tab.width * sizeof(*tab.coordinates_y[i]));
1468			if (ret) {
1469				dev_err(isp->dev,
1470					"Failed to copy to User for y\n");
1471				return -EFAULT;
1472			}
1473		}
1474	} else {
1475		struct ia_css_morph_table *tab =
1476			    asd->params.css_param.morph_table;
1477
1478		/* free first if we have one */
1479		if (tab) {
1480			atomisp_css_morph_table_free(tab);
1481			asd->params.css_param.morph_table = NULL;
1482		}
1483
1484		/* allocate new one */
1485		tab = atomisp_css_morph_table_allocate(config->width,
1486						       config->height);
1487
1488		if (!tab) {
1489			dev_err(isp->dev, "out of memory\n");
1490			return -EINVAL;
1491		}
1492
1493		for (i = 0; i < IA_CSS_MORPH_TABLE_NUM_PLANES; i++) {
1494			ret = copy_from_user(tab->coordinates_x[i],
1495					     config->coordinates_x[i],
1496					     config->height * config->width *
1497					     sizeof(*config->coordinates_x[i]));
1498			if (ret) {
1499				dev_err(isp->dev,
1500					"Failed to copy from User for x, ret %d\n",
1501					ret);
1502				atomisp_css_morph_table_free(tab);
1503				return -EFAULT;
1504			}
1505			ret = copy_from_user(tab->coordinates_y[i],
1506					     config->coordinates_y[i],
1507					     config->height * config->width *
1508					     sizeof(*config->coordinates_y[i]));
1509			if (ret) {
1510				dev_err(isp->dev,
1511					"Failed to copy from User for y, ret is %d\n",
1512					ret);
1513				atomisp_css_morph_table_free(tab);
1514				return -EFAULT;
1515			}
1516		}
1517		asd->params.css_param.morph_table = tab;
1518		if (asd->params.gdc_cac_en)
1519			asd->params.config.morph_table = tab;
1520	}
1521
1522	return 0;
1523}
1524
1525int atomisp_macc_table(struct atomisp_sub_device *asd, int flag,
1526		       struct atomisp_macc_config *config)
1527{
1528	struct ia_css_macc_table *macc_table;
1529
1530	switch (config->color_effect) {
1531	case V4L2_COLORFX_NONE:
1532		macc_table = &asd->params.css_param.macc_table;
1533		break;
1534	case V4L2_COLORFX_SKY_BLUE:
1535		macc_table = &blue_macc_table;
1536		break;
1537	case V4L2_COLORFX_GRASS_GREEN:
1538		macc_table = &green_macc_table;
1539		break;
1540	case V4L2_COLORFX_SKIN_WHITEN_LOW:
1541		macc_table = &skin_low_macc_table;
1542		break;
1543	case V4L2_COLORFX_SKIN_WHITEN:
1544		macc_table = &skin_medium_macc_table;
1545		break;
1546	case V4L2_COLORFX_SKIN_WHITEN_HIGH:
1547		macc_table = &skin_high_macc_table;
1548		break;
1549	default:
1550		return -EINVAL;
1551	}
1552
1553	if (flag == 0) {
1554		/* Get macc table from current setup */
1555		memcpy(&config->table, macc_table,
1556		       sizeof(struct ia_css_macc_table));
1557	} else {
1558		memcpy(macc_table, &config->table,
1559		       sizeof(struct ia_css_macc_table));
1560		if (config->color_effect == asd->params.color_effect)
1561			asd->params.config.macc_table = macc_table;
1562	}
1563
1564	return 0;
1565}
1566
1567int atomisp_set_dis_vector(struct atomisp_sub_device *asd,
1568			   struct atomisp_dis_vector *vector)
1569{
1570	atomisp_css_video_set_dis_vector(asd, vector);
1571
1572	asd->params.dis_proj_data_valid = false;
1573	asd->params.css_update_params_needed = true;
1574	return 0;
1575}
1576
1577/*
1578 * Function to set/get image stablization statistics
1579 */
1580int atomisp_get_dis_stat(struct atomisp_sub_device *asd,
1581			 struct atomisp_dis_statistics *stats)
1582{
1583	return atomisp_css_get_dis_stat(asd, stats);
1584}
1585
1586/*
1587 * Function  set camrea_prefiles.xml current sensor pixel array size
1588 */
1589int atomisp_set_array_res(struct atomisp_sub_device *asd,
1590			  struct atomisp_resolution  *config)
1591{
1592	dev_dbg(asd->isp->dev, ">%s start\n", __func__);
1593	if (!config) {
1594		dev_err(asd->isp->dev, "Set sensor array size is not valid\n");
1595		return -EINVAL;
1596	}
1597
1598	asd->sensor_array_res.width = config->width;
1599	asd->sensor_array_res.height = config->height;
1600	return 0;
1601}
1602
1603/*
1604 * Function to get DVS2 BQ resolution settings
1605 */
1606int atomisp_get_dvs2_bq_resolutions(struct atomisp_sub_device *asd,
1607				    struct atomisp_dvs2_bq_resolutions *bq_res)
1608{
1609	struct ia_css_pipe_config *pipe_cfg = NULL;
1610
1611	struct ia_css_stream *stream =
1612		    asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].stream;
1613	if (!stream) {
1614		dev_warn(asd->isp->dev, "stream is not created");
1615		return -EAGAIN;
1616	}
1617
1618	pipe_cfg = &asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL]
1619		   .pipe_configs[IA_CSS_PIPE_ID_VIDEO];
1620
1621	if (!bq_res)
1622		return -EINVAL;
1623
1624	/* the GDC output resolution */
1625	bq_res->output_bq.width_bq = pipe_cfg->output_info[0].res.width / 2;
1626	bq_res->output_bq.height_bq = pipe_cfg->output_info[0].res.height / 2;
1627
1628	bq_res->envelope_bq.width_bq = 0;
1629	bq_res->envelope_bq.height_bq = 0;
1630	/* the GDC input resolution */
1631	bq_res->source_bq.width_bq = bq_res->output_bq.width_bq +
1632				     pipe_cfg->dvs_envelope.width / 2;
1633	bq_res->source_bq.height_bq = bq_res->output_bq.height_bq +
1634				      pipe_cfg->dvs_envelope.height / 2;
1635	/*
1636	 * Bad pixels caused by spatial filter processing
1637	 * ISP filter resolution should be given by CSS/FW, but for now
1638	 * there is not such API to query, and it is fixed value, so
1639	 * hardcoded here.
1640	 */
1641	bq_res->ispfilter_bq.width_bq = 12 / 2;
1642	bq_res->ispfilter_bq.height_bq = 12 / 2;
1643	/* spatial filter shift, always 4 pixels */
1644	bq_res->gdc_shift_bq.width_bq = 4 / 2;
1645	bq_res->gdc_shift_bq.height_bq = 4 / 2;
1646
1647	if (asd->params.video_dis_en) {
1648		bq_res->envelope_bq.width_bq = pipe_cfg->dvs_envelope.width / 2 -
1649					       bq_res->ispfilter_bq.width_bq;
1650		bq_res->envelope_bq.height_bq = pipe_cfg->dvs_envelope.height / 2 -
1651						bq_res->ispfilter_bq.height_bq;
1652	}
1653
1654	dev_dbg(asd->isp->dev,
1655		"source_bq.width_bq %d, source_bq.height_bq %d,\nispfilter_bq.width_bq %d, ispfilter_bq.height_bq %d,\ngdc_shift_bq.width_bq %d, gdc_shift_bq.height_bq %d,\nenvelope_bq.width_bq %d, envelope_bq.height_bq %d,\noutput_bq.width_bq %d, output_bq.height_bq %d\n",
1656		bq_res->source_bq.width_bq, bq_res->source_bq.height_bq,
1657		bq_res->ispfilter_bq.width_bq, bq_res->ispfilter_bq.height_bq,
1658		bq_res->gdc_shift_bq.width_bq, bq_res->gdc_shift_bq.height_bq,
1659		bq_res->envelope_bq.width_bq, bq_res->envelope_bq.height_bq,
1660		bq_res->output_bq.width_bq, bq_res->output_bq.height_bq);
1661
1662	return 0;
1663}
1664
1665int atomisp_set_dis_coefs(struct atomisp_sub_device *asd,
1666			  struct atomisp_dis_coefficients *coefs)
1667{
1668	return atomisp_css_set_dis_coefs(asd, coefs);
1669}
1670
1671/*
1672 * Function to set/get 3A stat from isp
1673 */
1674int atomisp_3a_stat(struct atomisp_sub_device *asd, int flag,
1675		    struct atomisp_3a_statistics *config)
1676{
1677	struct atomisp_device *isp = asd->isp;
1678	struct atomisp_s3a_buf *s3a_buf;
1679	unsigned long ret;
1680
1681	if (flag != 0)
1682		return -EINVAL;
1683
1684	/* sanity check to avoid writing into unallocated memory. */
1685	if (asd->params.s3a_output_bytes == 0)
1686		return -EINVAL;
1687
1688	if (atomisp_compare_grid(asd, &config->grid_info) != 0) {
1689		/* If the grid info in the argument differs from the current
1690		   grid info, we tell the caller to reset the grid size and
1691		   try again. */
1692		return -EAGAIN;
1693	}
1694
1695	if (list_empty(&asd->s3a_stats_ready)) {
1696		dev_err(isp->dev, "3a statistics is not valid.\n");
1697		return -EAGAIN;
1698	}
1699
1700	s3a_buf = list_entry(asd->s3a_stats_ready.next,
1701			     struct atomisp_s3a_buf, list);
1702	if (s3a_buf->s3a_map)
1703		ia_css_translate_3a_statistics(
1704		    asd->params.s3a_user_stat, s3a_buf->s3a_map);
1705	else
1706		ia_css_get_3a_statistics(asd->params.s3a_user_stat,
1707					 s3a_buf->s3a_data);
1708
1709	config->exp_id = s3a_buf->s3a_data->exp_id;
1710	config->isp_config_id = s3a_buf->s3a_data->isp_config_id;
1711
1712	ret = copy_to_user(config->data, asd->params.s3a_user_stat->data,
1713			   asd->params.s3a_output_bytes);
1714	if (ret) {
1715		dev_err(isp->dev, "copy to user failed: copied %lu bytes\n",
1716			ret);
1717		return -EFAULT;
1718	}
1719
1720	/* Move to free buffer list */
1721	list_del_init(&s3a_buf->list);
1722	list_add_tail(&s3a_buf->list, &asd->s3a_stats);
1723	dev_dbg(isp->dev, "%s: finish getting exp_id %d 3a stat, isp_config_id %d\n",
1724		__func__,
1725		config->exp_id, config->isp_config_id);
1726	return 0;
1727}
1728
1729/*
1730 * Function to calculate real zoom region for every pipe
1731 */
1732int atomisp_calculate_real_zoom_region(struct atomisp_sub_device *asd,
1733				       struct ia_css_dz_config   *dz_config,
1734				       enum ia_css_pipe_id css_pipe_id)
1735
1736{
1737	struct atomisp_stream_env *stream_env =
1738		    &asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL];
1739	struct atomisp_resolution  eff_res, out_res;
1740	int w_offset, h_offset;
1741
1742	memset(&eff_res, 0, sizeof(eff_res));
1743	memset(&out_res, 0, sizeof(out_res));
1744
1745	if (dz_config->dx || dz_config->dy)
1746		return 0;
1747
1748	if (css_pipe_id != IA_CSS_PIPE_ID_PREVIEW
1749	    && css_pipe_id != IA_CSS_PIPE_ID_CAPTURE) {
1750		dev_err(asd->isp->dev, "%s the set pipe no support crop region"
1751			, __func__);
1752		return -EINVAL;
1753	}
1754
1755	eff_res.width =
1756	    stream_env->stream_config.input_config.effective_res.width;
1757	eff_res.height =
1758	    stream_env->stream_config.input_config.effective_res.height;
1759	if (eff_res.width == 0 || eff_res.height == 0) {
1760		dev_err(asd->isp->dev, "%s err effective resolution"
1761			, __func__);
1762		return -EINVAL;
1763	}
1764
1765	if (dz_config->zoom_region.resolution.width
1766	    == asd->sensor_array_res.width
1767	    || dz_config->zoom_region.resolution.height
1768	    == asd->sensor_array_res.height) {
1769		/*no need crop region*/
1770		dz_config->zoom_region.origin.x = 0;
1771		dz_config->zoom_region.origin.y = 0;
1772		dz_config->zoom_region.resolution.width = eff_res.width;
1773		dz_config->zoom_region.resolution.height = eff_res.height;
1774		return 0;
1775	}
1776
1777	/* FIXME:
1778	 * This is not the correct implementation with Google's definition, due
1779	 * to firmware limitation.
1780	 * map real crop region base on above calculating base max crop region.
1781	 */
1782
1783	if (!IS_ISP2401) {
1784		dz_config->zoom_region.origin.x = dz_config->zoom_region.origin.x
1785						  * eff_res.width
1786						  / asd->sensor_array_res.width;
1787		dz_config->zoom_region.origin.y = dz_config->zoom_region.origin.y
1788						  * eff_res.height
1789						  / asd->sensor_array_res.height;
1790		dz_config->zoom_region.resolution.width = dz_config->zoom_region.resolution.width
1791							  * eff_res.width
1792							  / asd->sensor_array_res.width;
1793		dz_config->zoom_region.resolution.height = dz_config->zoom_region.resolution.height
1794							  * eff_res.height
1795							  / asd->sensor_array_res.height;
1796		/*
1797		 * Set same ratio of crop region resolution and current pipe output
1798		 * resolution
1799		 */
1800		out_res.width = stream_env->pipe_configs[css_pipe_id].output_info[0].res.width;
1801		out_res.height = stream_env->pipe_configs[css_pipe_id].output_info[0].res.height;
1802		if (out_res.width == 0 || out_res.height == 0) {
1803			dev_err(asd->isp->dev, "%s err current pipe output resolution"
1804				, __func__);
1805			return -EINVAL;
1806		}
1807	} else {
1808		out_res.width = stream_env->pipe_configs[css_pipe_id].output_info[0].res.width;
1809		out_res.height = stream_env->pipe_configs[css_pipe_id].output_info[0].res.height;
1810		if (out_res.width == 0 || out_res.height == 0) {
1811			dev_err(asd->isp->dev, "%s err current pipe output resolution"
1812				, __func__);
1813			return -EINVAL;
1814		}
1815
1816		if (asd->sensor_array_res.width * out_res.height
1817		    < out_res.width * asd->sensor_array_res.height) {
1818			h_offset = asd->sensor_array_res.height
1819				   - asd->sensor_array_res.width
1820				   * out_res.height / out_res.width;
1821			h_offset = h_offset / 2;
1822			if (dz_config->zoom_region.origin.y < h_offset)
1823				dz_config->zoom_region.origin.y = 0;
1824			else
1825				dz_config->zoom_region.origin.y = dz_config->zoom_region.origin.y - h_offset;
1826			w_offset = 0;
1827		} else {
1828			w_offset = asd->sensor_array_res.width
1829				   - asd->sensor_array_res.height
1830				   * out_res.width / out_res.height;
1831			w_offset = w_offset / 2;
1832			if (dz_config->zoom_region.origin.x < w_offset)
1833				dz_config->zoom_region.origin.x = 0;
1834			else
1835				dz_config->zoom_region.origin.x = dz_config->zoom_region.origin.x - w_offset;
1836			h_offset = 0;
1837		}
1838		dz_config->zoom_region.origin.x = dz_config->zoom_region.origin.x
1839						  * eff_res.width
1840						  / (asd->sensor_array_res.width - 2 * w_offset);
1841		dz_config->zoom_region.origin.y = dz_config->zoom_region.origin.y
1842						  * eff_res.height
1843						  / (asd->sensor_array_res.height - 2 * h_offset);
1844		dz_config->zoom_region.resolution.width = dz_config->zoom_region.resolution.width
1845						  * eff_res.width
1846						  / (asd->sensor_array_res.width - 2 * w_offset);
1847		dz_config->zoom_region.resolution.height = dz_config->zoom_region.resolution.height
1848						  * eff_res.height
1849						  / (asd->sensor_array_res.height - 2 * h_offset);
1850	}
1851
1852	if (out_res.width * dz_config->zoom_region.resolution.height
1853	    > dz_config->zoom_region.resolution.width * out_res.height) {
1854		dz_config->zoom_region.resolution.height =
1855		    dz_config->zoom_region.resolution.width
1856		    * out_res.height / out_res.width;
1857	} else {
1858		dz_config->zoom_region.resolution.width =
1859		    dz_config->zoom_region.resolution.height
1860		    * out_res.width / out_res.height;
1861	}
1862	dev_dbg(asd->isp->dev,
1863		"%s crop region:(%d,%d),(%d,%d) eff_res(%d, %d) array_size(%d,%d) out_res(%d, %d)\n",
1864		__func__, dz_config->zoom_region.origin.x,
1865		dz_config->zoom_region.origin.y,
1866		dz_config->zoom_region.resolution.width,
1867		dz_config->zoom_region.resolution.height,
1868		eff_res.width, eff_res.height,
1869		asd->sensor_array_res.width,
1870		asd->sensor_array_res.height,
1871		out_res.width, out_res.height);
1872
1873	if ((dz_config->zoom_region.origin.x +
1874	     dz_config->zoom_region.resolution.width
1875	     > eff_res.width) ||
1876	    (dz_config->zoom_region.origin.y +
1877	     dz_config->zoom_region.resolution.height
1878	     > eff_res.height))
1879		return -EINVAL;
1880
1881	return 0;
1882}
1883
1884/*
1885 * Function to check the zoom region whether is effective
1886 */
1887static bool atomisp_check_zoom_region(
1888    struct atomisp_sub_device *asd,
1889    struct ia_css_dz_config *dz_config)
1890{
1891	struct atomisp_resolution  config;
1892	bool flag = false;
1893	unsigned int w, h;
1894
1895	memset(&config, 0, sizeof(struct atomisp_resolution));
1896
1897	if (dz_config->dx && dz_config->dy)
1898		return true;
1899
1900	config.width = asd->sensor_array_res.width;
1901	config.height = asd->sensor_array_res.height;
1902	w = dz_config->zoom_region.origin.x +
1903	    dz_config->zoom_region.resolution.width;
1904	h = dz_config->zoom_region.origin.y +
1905	    dz_config->zoom_region.resolution.height;
1906
1907	if ((w <= config.width) && (h <= config.height) && w > 0 && h > 0)
1908		flag = true;
1909	else
1910		/* setting error zoom region */
1911		dev_err(asd->isp->dev,
1912			"%s zoom region ERROR:dz_config:(%d,%d),(%d,%d)array_res(%d, %d)\n",
1913			__func__, dz_config->zoom_region.origin.x,
1914			dz_config->zoom_region.origin.y,
1915			dz_config->zoom_region.resolution.width,
1916			dz_config->zoom_region.resolution.height,
1917			config.width, config.height);
1918
1919	return flag;
1920}
1921
1922void atomisp_apply_css_parameters(
1923    struct atomisp_sub_device *asd,
1924    struct atomisp_css_params *css_param)
1925{
1926	if (css_param->update_flag.wb_config)
1927		asd->params.config.wb_config = &css_param->wb_config;
1928
1929	if (css_param->update_flag.ob_config)
1930		asd->params.config.ob_config = &css_param->ob_config;
1931
1932	if (css_param->update_flag.dp_config)
1933		asd->params.config.dp_config = &css_param->dp_config;
1934
1935	if (css_param->update_flag.nr_config)
1936		asd->params.config.nr_config = &css_param->nr_config;
1937
1938	if (css_param->update_flag.ee_config)
1939		asd->params.config.ee_config = &css_param->ee_config;
1940
1941	if (css_param->update_flag.tnr_config)
1942		asd->params.config.tnr_config = &css_param->tnr_config;
1943
1944	if (css_param->update_flag.a3a_config)
1945		asd->params.config.s3a_config = &css_param->s3a_config;
1946
1947	if (css_param->update_flag.ctc_config)
1948		asd->params.config.ctc_config = &css_param->ctc_config;
1949
1950	if (css_param->update_flag.cnr_config)
1951		asd->params.config.cnr_config = &css_param->cnr_config;
1952
1953	if (css_param->update_flag.ecd_config)
1954		asd->params.config.ecd_config = &css_param->ecd_config;
1955
1956	if (css_param->update_flag.ynr_config)
1957		asd->params.config.ynr_config = &css_param->ynr_config;
1958
1959	if (css_param->update_flag.fc_config)
1960		asd->params.config.fc_config = &css_param->fc_config;
1961
1962	if (css_param->update_flag.macc_config)
1963		asd->params.config.macc_config = &css_param->macc_config;
1964
1965	if (css_param->update_flag.aa_config)
1966		asd->params.config.aa_config = &css_param->aa_config;
1967
1968	if (css_param->update_flag.anr_config)
1969		asd->params.config.anr_config = &css_param->anr_config;
1970
1971	if (css_param->update_flag.xnr_config)
1972		asd->params.config.xnr_config = &css_param->xnr_config;
1973
1974	if (css_param->update_flag.yuv2rgb_cc_config)
1975		asd->params.config.yuv2rgb_cc_config = &css_param->yuv2rgb_cc_config;
1976
1977	if (css_param->update_flag.rgb2yuv_cc_config)
1978		asd->params.config.rgb2yuv_cc_config = &css_param->rgb2yuv_cc_config;
1979
1980	if (css_param->update_flag.macc_table)
1981		asd->params.config.macc_table = &css_param->macc_table;
1982
1983	if (css_param->update_flag.xnr_table)
1984		asd->params.config.xnr_table = &css_param->xnr_table;
1985
1986	if (css_param->update_flag.r_gamma_table)
1987		asd->params.config.r_gamma_table = &css_param->r_gamma_table;
1988
1989	if (css_param->update_flag.g_gamma_table)
1990		asd->params.config.g_gamma_table = &css_param->g_gamma_table;
1991
1992	if (css_param->update_flag.b_gamma_table)
1993		asd->params.config.b_gamma_table = &css_param->b_gamma_table;
1994
1995	if (css_param->update_flag.anr_thres)
1996		atomisp_css_set_anr_thres(asd, &css_param->anr_thres);
1997
1998	if (css_param->update_flag.shading_table)
1999		asd->params.config.shading_table = css_param->shading_table;
2000
2001	if (css_param->update_flag.morph_table && asd->params.gdc_cac_en)
2002		asd->params.config.morph_table = css_param->morph_table;
2003
2004	if (css_param->update_flag.dvs2_coefs) {
2005		struct ia_css_dvs_grid_info *dvs_grid_info =
2006		    atomisp_css_get_dvs_grid_info(
2007			&asd->params.curr_grid_info);
2008
2009		if (dvs_grid_info && dvs_grid_info->enable)
2010			atomisp_css_set_dvs2_coefs(asd, css_param->dvs2_coeff);
2011	}
2012
2013	if (css_param->update_flag.dvs_6axis_config)
2014		atomisp_css_set_dvs_6axis(asd, css_param->dvs_6axis);
2015
2016	atomisp_css_set_isp_config_id(asd, css_param->isp_config_id);
2017	/*
2018	 * These configurations are on used by ISP1.x, not for ISP2.x,
2019	 * so do not handle them. see comments of ia_css_isp_config.
2020	 * 1 cc_config
2021	 * 2 ce_config
2022	 * 3 de_config
2023	 * 4 gc_config
2024	 * 5 gamma_table
2025	 * 6 ctc_table
2026	 * 7 dvs_coefs
2027	 */
2028}
2029
2030static unsigned int long copy_from_compatible(void *to, const void *from,
2031	unsigned long n, bool from_user)
2032{
2033	if (from_user)
2034		return copy_from_user(to, (void __user *)from, n);
2035	else
2036		memcpy(to, from, n);
2037	return 0;
2038}
2039
2040int atomisp_cp_general_isp_parameters(struct atomisp_sub_device *asd,
2041				      struct atomisp_parameters *arg,
2042				      struct atomisp_css_params *css_param,
2043				      bool from_user)
2044{
2045	struct atomisp_parameters *cur_config = &css_param->update_flag;
2046
2047	if (!arg || !asd || !css_param)
2048		return -EINVAL;
2049
2050	if (arg->wb_config && (from_user || !cur_config->wb_config)) {
2051		if (copy_from_compatible(&css_param->wb_config, arg->wb_config,
2052					 sizeof(struct ia_css_wb_config),
2053					 from_user))
2054			return -EFAULT;
2055		css_param->update_flag.wb_config =
2056		    (struct atomisp_wb_config *)&css_param->wb_config;
2057	}
2058
2059	if (arg->ob_config && (from_user || !cur_config->ob_config)) {
2060		if (copy_from_compatible(&css_param->ob_config, arg->ob_config,
2061					 sizeof(struct ia_css_ob_config),
2062					 from_user))
2063			return -EFAULT;
2064		css_param->update_flag.ob_config =
2065		    (struct atomisp_ob_config *)&css_param->ob_config;
2066	}
2067
2068	if (arg->dp_config && (from_user || !cur_config->dp_config)) {
2069		if (copy_from_compatible(&css_param->dp_config, arg->dp_config,
2070					 sizeof(struct ia_css_dp_config),
2071					 from_user))
2072			return -EFAULT;
2073		css_param->update_flag.dp_config =
2074		    (struct atomisp_dp_config *)&css_param->dp_config;
2075	}
2076
2077	if (asd->run_mode->val != ATOMISP_RUN_MODE_VIDEO) {
2078		if (arg->dz_config && (from_user || !cur_config->dz_config)) {
2079			if (copy_from_compatible(&css_param->dz_config,
2080						 arg->dz_config,
2081						 sizeof(struct ia_css_dz_config),
2082						 from_user))
2083				return -EFAULT;
2084			if (!atomisp_check_zoom_region(asd,
2085						       &css_param->dz_config)) {
2086				dev_err(asd->isp->dev, "crop region error!");
2087				return -EINVAL;
2088			}
2089			css_param->update_flag.dz_config =
2090			    (struct atomisp_dz_config *)
2091			    &css_param->dz_config;
2092		}
2093	}
2094
2095	if (arg->nr_config && (from_user || !cur_config->nr_config)) {
2096		if (copy_from_compatible(&css_param->nr_config, arg->nr_config,
2097					 sizeof(struct ia_css_nr_config),
2098					 from_user))
2099			return -EFAULT;
2100		css_param->update_flag.nr_config =
2101		    (struct atomisp_nr_config *)&css_param->nr_config;
2102	}
2103
2104	if (arg->ee_config && (from_user || !cur_config->ee_config)) {
2105		if (copy_from_compatible(&css_param->ee_config, arg->ee_config,
2106					 sizeof(struct ia_css_ee_config),
2107					 from_user))
2108			return -EFAULT;
2109		css_param->update_flag.ee_config =
2110		    (struct atomisp_ee_config *)&css_param->ee_config;
2111	}
2112
2113	if (arg->tnr_config && (from_user || !cur_config->tnr_config)) {
2114		if (copy_from_compatible(&css_param->tnr_config,
2115					 arg->tnr_config,
2116					 sizeof(struct ia_css_tnr_config),
2117					 from_user))
2118			return -EFAULT;
2119		css_param->update_flag.tnr_config =
2120		    (struct atomisp_tnr_config *)
2121		    &css_param->tnr_config;
2122	}
2123
2124	if (arg->a3a_config && (from_user || !cur_config->a3a_config)) {
2125		if (copy_from_compatible(&css_param->s3a_config,
2126					 arg->a3a_config,
2127					 sizeof(struct ia_css_3a_config),
2128					 from_user))
2129			return -EFAULT;
2130		css_param->update_flag.a3a_config =
2131		    (struct atomisp_3a_config *)&css_param->s3a_config;
2132	}
2133
2134	if (arg->ctc_config && (from_user || !cur_config->ctc_config)) {
2135		if (copy_from_compatible(&css_param->ctc_config,
2136					 arg->ctc_config,
2137					 sizeof(struct ia_css_ctc_config),
2138					 from_user))
2139			return -EFAULT;
2140		css_param->update_flag.ctc_config =
2141		    (struct atomisp_ctc_config *)
2142		    &css_param->ctc_config;
2143	}
2144
2145	if (arg->cnr_config && (from_user || !cur_config->cnr_config)) {
2146		if (copy_from_compatible(&css_param->cnr_config,
2147					 arg->cnr_config,
2148					 sizeof(struct ia_css_cnr_config),
2149					 from_user))
2150			return -EFAULT;
2151		css_param->update_flag.cnr_config =
2152		    (struct atomisp_cnr_config *)
2153		    &css_param->cnr_config;
2154	}
2155
2156	if (arg->ecd_config && (from_user || !cur_config->ecd_config)) {
2157		if (copy_from_compatible(&css_param->ecd_config,
2158					 arg->ecd_config,
2159					 sizeof(struct ia_css_ecd_config),
2160					 from_user))
2161			return -EFAULT;
2162		css_param->update_flag.ecd_config =
2163		    (struct atomisp_ecd_config *)
2164		    &css_param->ecd_config;
2165	}
2166
2167	if (arg->ynr_config && (from_user || !cur_config->ynr_config)) {
2168		if (copy_from_compatible(&css_param->ynr_config,
2169					 arg->ynr_config,
2170					 sizeof(struct ia_css_ynr_config),
2171					 from_user))
2172			return -EFAULT;
2173		css_param->update_flag.ynr_config =
2174		    (struct atomisp_ynr_config *)
2175		    &css_param->ynr_config;
2176	}
2177
2178	if (arg->fc_config && (from_user || !cur_config->fc_config)) {
2179		if (copy_from_compatible(&css_param->fc_config,
2180					 arg->fc_config,
2181					 sizeof(struct ia_css_fc_config),
2182					 from_user))
2183			return -EFAULT;
2184		css_param->update_flag.fc_config =
2185		    (struct atomisp_fc_config *)&css_param->fc_config;
2186	}
2187
2188	if (arg->macc_config && (from_user || !cur_config->macc_config)) {
2189		if (copy_from_compatible(&css_param->macc_config,
2190					 arg->macc_config,
2191					 sizeof(struct ia_css_macc_config),
2192					 from_user))
2193			return -EFAULT;
2194		css_param->update_flag.macc_config =
2195		    (struct atomisp_macc_config *)
2196		    &css_param->macc_config;
2197	}
2198
2199	if (arg->aa_config && (from_user || !cur_config->aa_config)) {
2200		if (copy_from_compatible(&css_param->aa_config, arg->aa_config,
2201					 sizeof(struct ia_css_aa_config),
2202					 from_user))
2203			return -EFAULT;
2204		css_param->update_flag.aa_config =
2205		    (struct atomisp_aa_config *)&css_param->aa_config;
2206	}
2207
2208	if (arg->anr_config && (from_user || !cur_config->anr_config)) {
2209		if (copy_from_compatible(&css_param->anr_config,
2210					 arg->anr_config,
2211					 sizeof(struct ia_css_anr_config),
2212					 from_user))
2213			return -EFAULT;
2214		css_param->update_flag.anr_config =
2215		    (struct atomisp_anr_config *)
2216		    &css_param->anr_config;
2217	}
2218
2219	if (arg->xnr_config && (from_user || !cur_config->xnr_config)) {
2220		if (copy_from_compatible(&css_param->xnr_config,
2221					 arg->xnr_config,
2222					 sizeof(struct ia_css_xnr_config),
2223					 from_user))
2224			return -EFAULT;
2225		css_param->update_flag.xnr_config =
2226		    (struct atomisp_xnr_config *)
2227		    &css_param->xnr_config;
2228	}
2229
2230	if (arg->yuv2rgb_cc_config &&
2231	    (from_user || !cur_config->yuv2rgb_cc_config)) {
2232		if (copy_from_compatible(&css_param->yuv2rgb_cc_config,
2233					 arg->yuv2rgb_cc_config,
2234					 sizeof(struct ia_css_cc_config),
2235					 from_user))
2236			return -EFAULT;
2237		css_param->update_flag.yuv2rgb_cc_config =
2238		    (struct atomisp_cc_config *)
2239		    &css_param->yuv2rgb_cc_config;
2240	}
2241
2242	if (arg->rgb2yuv_cc_config &&
2243	    (from_user || !cur_config->rgb2yuv_cc_config)) {
2244		if (copy_from_compatible(&css_param->rgb2yuv_cc_config,
2245					 arg->rgb2yuv_cc_config,
2246					 sizeof(struct ia_css_cc_config),
2247					 from_user))
2248			return -EFAULT;
2249		css_param->update_flag.rgb2yuv_cc_config =
2250		    (struct atomisp_cc_config *)
2251		    &css_param->rgb2yuv_cc_config;
2252	}
2253
2254	if (arg->macc_table && (from_user || !cur_config->macc_table)) {
2255		if (copy_from_compatible(&css_param->macc_table,
2256					 arg->macc_table,
2257					 sizeof(struct ia_css_macc_table),
2258					 from_user))
2259			return -EFAULT;
2260		css_param->update_flag.macc_table =
2261		    (struct atomisp_macc_table *)
2262		    &css_param->macc_table;
2263	}
2264
2265	if (arg->xnr_table && (from_user || !cur_config->xnr_table)) {
2266		if (copy_from_compatible(&css_param->xnr_table,
2267					 arg->xnr_table,
2268					 sizeof(struct ia_css_xnr_table),
2269					 from_user))
2270			return -EFAULT;
2271		css_param->update_flag.xnr_table =
2272		    (struct atomisp_xnr_table *)&css_param->xnr_table;
2273	}
2274
2275	if (arg->r_gamma_table && (from_user || !cur_config->r_gamma_table)) {
2276		if (copy_from_compatible(&css_param->r_gamma_table,
2277					 arg->r_gamma_table,
2278					 sizeof(struct ia_css_rgb_gamma_table),
2279					 from_user))
2280			return -EFAULT;
2281		css_param->update_flag.r_gamma_table =
2282		    (struct atomisp_rgb_gamma_table *)
2283		    &css_param->r_gamma_table;
2284	}
2285
2286	if (arg->g_gamma_table && (from_user || !cur_config->g_gamma_table)) {
2287		if (copy_from_compatible(&css_param->g_gamma_table,
2288					 arg->g_gamma_table,
2289					 sizeof(struct ia_css_rgb_gamma_table),
2290					 from_user))
2291			return -EFAULT;
2292		css_param->update_flag.g_gamma_table =
2293		    (struct atomisp_rgb_gamma_table *)
2294		    &css_param->g_gamma_table;
2295	}
2296
2297	if (arg->b_gamma_table && (from_user || !cur_config->b_gamma_table)) {
2298		if (copy_from_compatible(&css_param->b_gamma_table,
2299					 arg->b_gamma_table,
2300					 sizeof(struct ia_css_rgb_gamma_table),
2301					 from_user))
2302			return -EFAULT;
2303		css_param->update_flag.b_gamma_table =
2304		    (struct atomisp_rgb_gamma_table *)
2305		    &css_param->b_gamma_table;
2306	}
2307
2308	if (arg->anr_thres && (from_user || !cur_config->anr_thres)) {
2309		if (copy_from_compatible(&css_param->anr_thres, arg->anr_thres,
2310					 sizeof(struct ia_css_anr_thres),
2311					 from_user))
2312			return -EFAULT;
2313		css_param->update_flag.anr_thres =
2314		    (struct atomisp_anr_thres *)&css_param->anr_thres;
2315	}
2316
2317	if (from_user)
2318		css_param->isp_config_id = arg->isp_config_id;
2319	/*
2320	 * These configurations are on used by ISP1.x, not for ISP2.x,
2321	 * so do not handle them. see comments of ia_css_isp_config.
2322	 * 1 cc_config
2323	 * 2 ce_config
2324	 * 3 de_config
2325	 * 4 gc_config
2326	 * 5 gamma_table
2327	 * 6 ctc_table
2328	 * 7 dvs_coefs
2329	 */
2330	return 0;
2331}
2332
2333int atomisp_cp_lsc_table(struct atomisp_sub_device *asd,
2334			 struct atomisp_shading_table *source_st,
2335			 struct atomisp_css_params *css_param,
2336			 bool from_user)
2337{
2338	unsigned int i;
2339	unsigned int len_table;
2340	struct ia_css_shading_table *shading_table;
2341	struct ia_css_shading_table *old_table;
2342	struct atomisp_shading_table *st, dest_st;
2343
2344	if (!source_st)
2345		return 0;
2346
2347	if (!css_param)
2348		return -EINVAL;
2349
2350	if (!from_user && css_param->update_flag.shading_table)
2351		return 0;
2352
2353	if (IS_ISP2401) {
2354		if (copy_from_compatible(&dest_st, source_st,
2355					sizeof(struct atomisp_shading_table),
2356					from_user)) {
2357			dev_err(asd->isp->dev, "copy shading table failed!");
2358			return -EFAULT;
2359		}
2360		st = &dest_st;
2361	} else {
2362		st = source_st;
2363	}
2364
2365	old_table = css_param->shading_table;
2366
2367	/* user config is to disable the shading table. */
2368	if (!st->enable) {
2369		/* Generate a minimum table with enable = 0. */
2370		shading_table = atomisp_css_shading_table_alloc(1, 1);
2371		if (!shading_table)
2372			return -ENOMEM;
2373		shading_table->enable = 0;
2374		goto set_lsc;
2375	}
2376
2377	/* Setting a new table. Validate first - all tables must be set */
2378	for (i = 0; i < ATOMISP_NUM_SC_COLORS; i++) {
2379		if (!st->data[i]) {
2380			dev_err(asd->isp->dev, "shading table validate failed");
2381			return -EINVAL;
2382		}
2383	}
2384
2385	/* Shading table size per color */
2386	if (st->width > SH_CSS_MAX_SCTBL_WIDTH_PER_COLOR ||
2387	    st->height > SH_CSS_MAX_SCTBL_HEIGHT_PER_COLOR) {
2388		dev_err(asd->isp->dev, "shading table w/h validate failed!");
2389		return -EINVAL;
2390	}
2391
2392	shading_table = atomisp_css_shading_table_alloc(st->width, st->height);
2393	if (!shading_table)
2394		return -ENOMEM;
2395
2396	len_table = st->width * st->height * ATOMISP_SC_TYPE_SIZE;
2397	for (i = 0; i < ATOMISP_NUM_SC_COLORS; i++) {
2398		if (copy_from_compatible(shading_table->data[i],
2399					 st->data[i], len_table, from_user)) {
2400			atomisp_css_shading_table_free(shading_table);
2401			return -EFAULT;
2402		}
2403	}
2404	shading_table->sensor_width = st->sensor_width;
2405	shading_table->sensor_height = st->sensor_height;
2406	shading_table->fraction_bits = st->fraction_bits;
2407	shading_table->enable = st->enable;
2408
2409	/* No need to update shading table if it is the same */
2410	if (old_table &&
2411	    old_table->sensor_width == shading_table->sensor_width &&
2412	    old_table->sensor_height == shading_table->sensor_height &&
2413	    old_table->width == shading_table->width &&
2414	    old_table->height == shading_table->height &&
2415	    old_table->fraction_bits == shading_table->fraction_bits &&
2416	    old_table->enable == shading_table->enable) {
2417		bool data_is_same = true;
2418
2419		for (i = 0; i < ATOMISP_NUM_SC_COLORS; i++) {
2420			if (memcmp(shading_table->data[i], old_table->data[i],
2421				   len_table) != 0) {
2422				data_is_same = false;
2423				break;
2424			}
2425		}
2426
2427		if (data_is_same) {
2428			atomisp_css_shading_table_free(shading_table);
2429			return 0;
2430		}
2431	}
2432
2433set_lsc:
2434	/* set LSC to CSS */
2435	css_param->shading_table = shading_table;
2436	css_param->update_flag.shading_table = (struct atomisp_shading_table *)shading_table;
2437	asd->params.sc_en = shading_table;
2438
2439	if (old_table)
2440		atomisp_css_shading_table_free(old_table);
2441
2442	return 0;
2443}
2444
2445int atomisp_css_cp_dvs2_coefs(struct atomisp_sub_device *asd,
2446			      struct ia_css_dvs2_coefficients *coefs,
2447			      struct atomisp_css_params *css_param,
2448			      bool from_user)
2449{
2450	struct ia_css_dvs_grid_info *cur =
2451	    atomisp_css_get_dvs_grid_info(&asd->params.curr_grid_info);
2452	int dvs_hor_coef_bytes, dvs_ver_coef_bytes;
2453	struct ia_css_dvs2_coefficients dvs2_coefs;
2454
2455	if (!coefs || !cur)
2456		return 0;
2457
2458	if (!from_user && css_param->update_flag.dvs2_coefs)
2459		return 0;
2460
2461	if (!IS_ISP2401) {
2462		if (sizeof(*cur) != sizeof(coefs->grid) ||
2463		    memcmp(&coefs->grid, cur, sizeof(coefs->grid))) {
2464			dev_err(asd->isp->dev, "dvs grid mismatch!\n");
2465			/* If the grid info in the argument differs from the current
2466			grid info, we tell the caller to reset the grid size and
2467			try again. */
2468			return -EAGAIN;
2469		}
2470
2471		if (!coefs->hor_coefs.odd_real ||
2472		    !coefs->hor_coefs.odd_imag ||
2473		    !coefs->hor_coefs.even_real ||
2474		    !coefs->hor_coefs.even_imag ||
2475		    !coefs->ver_coefs.odd_real ||
2476		    !coefs->ver_coefs.odd_imag ||
2477		    !coefs->ver_coefs.even_real ||
2478		    !coefs->ver_coefs.even_imag)
2479			return -EINVAL;
2480
2481		if (!css_param->dvs2_coeff) {
2482			/* DIS coefficients. */
2483			css_param->dvs2_coeff = ia_css_dvs2_coefficients_allocate(cur);
2484			if (!css_param->dvs2_coeff)
2485				return -ENOMEM;
2486		}
2487
2488		dvs_hor_coef_bytes = asd->params.dvs_hor_coef_bytes;
2489		dvs_ver_coef_bytes = asd->params.dvs_ver_coef_bytes;
2490		if (copy_from_compatible(css_param->dvs2_coeff->hor_coefs.odd_real,
2491					coefs->hor_coefs.odd_real, dvs_hor_coef_bytes, from_user) ||
2492		    copy_from_compatible(css_param->dvs2_coeff->hor_coefs.odd_imag,
2493					coefs->hor_coefs.odd_imag, dvs_hor_coef_bytes, from_user) ||
2494		    copy_from_compatible(css_param->dvs2_coeff->hor_coefs.even_real,
2495					coefs->hor_coefs.even_real, dvs_hor_coef_bytes, from_user) ||
2496		    copy_from_compatible(css_param->dvs2_coeff->hor_coefs.even_imag,
2497					coefs->hor_coefs.even_imag, dvs_hor_coef_bytes, from_user) ||
2498		    copy_from_compatible(css_param->dvs2_coeff->ver_coefs.odd_real,
2499					coefs->ver_coefs.odd_real, dvs_ver_coef_bytes, from_user) ||
2500		    copy_from_compatible(css_param->dvs2_coeff->ver_coefs.odd_imag,
2501					coefs->ver_coefs.odd_imag, dvs_ver_coef_bytes, from_user) ||
2502		    copy_from_compatible(css_param->dvs2_coeff->ver_coefs.even_real,
2503					coefs->ver_coefs.even_real, dvs_ver_coef_bytes, from_user) ||
2504		    copy_from_compatible(css_param->dvs2_coeff->ver_coefs.even_imag,
2505					coefs->ver_coefs.even_imag, dvs_ver_coef_bytes, from_user)) {
2506			ia_css_dvs2_coefficients_free(css_param->dvs2_coeff);
2507			css_param->dvs2_coeff = NULL;
2508			return -EFAULT;
2509		}
2510	} else {
2511		if (copy_from_compatible(&dvs2_coefs, coefs,
2512					sizeof(struct ia_css_dvs2_coefficients),
2513					from_user)) {
2514			dev_err(asd->isp->dev, "copy dvs2 coef failed");
2515			return -EFAULT;
2516		}
2517
2518		if (sizeof(*cur) != sizeof(dvs2_coefs.grid) ||
2519		    memcmp(&dvs2_coefs.grid, cur, sizeof(dvs2_coefs.grid))) {
2520			dev_err(asd->isp->dev, "dvs grid mismatch!\n");
2521			/* If the grid info in the argument differs from the current
2522			grid info, we tell the caller to reset the grid size and
2523			try again. */
2524			return -EAGAIN;
2525		}
2526
2527		if (!dvs2_coefs.hor_coefs.odd_real ||
2528		    !dvs2_coefs.hor_coefs.odd_imag ||
2529		    !dvs2_coefs.hor_coefs.even_real ||
2530		    !dvs2_coefs.hor_coefs.even_imag ||
2531		    !dvs2_coefs.ver_coefs.odd_real ||
2532		    !dvs2_coefs.ver_coefs.odd_imag ||
2533		    !dvs2_coefs.ver_coefs.even_real ||
2534		    !dvs2_coefs.ver_coefs.even_imag)
2535			return -EINVAL;
2536
2537		if (!css_param->dvs2_coeff) {
2538			/* DIS coefficients. */
2539			css_param->dvs2_coeff = ia_css_dvs2_coefficients_allocate(cur);
2540			if (!css_param->dvs2_coeff)
2541				return -ENOMEM;
2542		}
2543
2544		dvs_hor_coef_bytes = asd->params.dvs_hor_coef_bytes;
2545		dvs_ver_coef_bytes = asd->params.dvs_ver_coef_bytes;
2546		if (copy_from_compatible(css_param->dvs2_coeff->hor_coefs.odd_real,
2547					dvs2_coefs.hor_coefs.odd_real, dvs_hor_coef_bytes, from_user) ||
2548		    copy_from_compatible(css_param->dvs2_coeff->hor_coefs.odd_imag,
2549					dvs2_coefs.hor_coefs.odd_imag, dvs_hor_coef_bytes, from_user) ||
2550		    copy_from_compatible(css_param->dvs2_coeff->hor_coefs.even_real,
2551					dvs2_coefs.hor_coefs.even_real, dvs_hor_coef_bytes, from_user) ||
2552		    copy_from_compatible(css_param->dvs2_coeff->hor_coefs.even_imag,
2553					dvs2_coefs.hor_coefs.even_imag, dvs_hor_coef_bytes, from_user) ||
2554		    copy_from_compatible(css_param->dvs2_coeff->ver_coefs.odd_real,
2555					dvs2_coefs.ver_coefs.odd_real, dvs_ver_coef_bytes, from_user) ||
2556		    copy_from_compatible(css_param->dvs2_coeff->ver_coefs.odd_imag,
2557					dvs2_coefs.ver_coefs.odd_imag, dvs_ver_coef_bytes, from_user) ||
2558		    copy_from_compatible(css_param->dvs2_coeff->ver_coefs.even_real,
2559					dvs2_coefs.ver_coefs.even_real, dvs_ver_coef_bytes, from_user) ||
2560		    copy_from_compatible(css_param->dvs2_coeff->ver_coefs.even_imag,
2561					dvs2_coefs.ver_coefs.even_imag, dvs_ver_coef_bytes, from_user)) {
2562			ia_css_dvs2_coefficients_free(css_param->dvs2_coeff);
2563			css_param->dvs2_coeff = NULL;
2564			return -EFAULT;
2565		}
2566	}
2567
2568	css_param->update_flag.dvs2_coefs =
2569	    (struct atomisp_dis_coefficients *)css_param->dvs2_coeff;
2570	return 0;
2571}
2572
2573int atomisp_cp_dvs_6axis_config(struct atomisp_sub_device *asd,
2574				struct atomisp_dvs_6axis_config *source_6axis_config,
2575				struct atomisp_css_params *css_param,
2576				bool from_user)
2577{
2578	struct ia_css_dvs_6axis_config *dvs_6axis_config;
2579	struct ia_css_dvs_6axis_config *old_6axis_config;
2580	struct ia_css_stream *stream =
2581		    asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].stream;
2582	struct ia_css_dvs_grid_info *dvs_grid_info =
2583	    atomisp_css_get_dvs_grid_info(&asd->params.curr_grid_info);
2584	int ret = -EFAULT;
2585
2586	if (!stream) {
2587		dev_err(asd->isp->dev, "%s: internal error!", __func__);
2588		return -EINVAL;
2589	}
2590
2591	if (!source_6axis_config || !dvs_grid_info)
2592		return 0;
2593
2594	if (!dvs_grid_info->enable)
2595		return 0;
2596
2597	if (!from_user && css_param->update_flag.dvs_6axis_config)
2598		return 0;
2599
2600	/* check whether need to reallocate for 6 axis config */
2601	old_6axis_config = css_param->dvs_6axis;
2602	dvs_6axis_config = old_6axis_config;
2603
2604	if (IS_ISP2401) {
2605		struct ia_css_dvs_6axis_config t_6axis_config;
2606
2607		if (copy_from_compatible(&t_6axis_config, source_6axis_config,
2608					sizeof(struct atomisp_dvs_6axis_config),
2609					from_user)) {
2610			dev_err(asd->isp->dev, "copy morph table failed!");
2611			return -EFAULT;
2612		}
2613
2614		if (old_6axis_config &&
2615		    (old_6axis_config->width_y != t_6axis_config.width_y ||
2616		    old_6axis_config->height_y != t_6axis_config.height_y ||
2617		    old_6axis_config->width_uv != t_6axis_config.width_uv ||
2618		    old_6axis_config->height_uv != t_6axis_config.height_uv)) {
2619			ia_css_dvs2_6axis_config_free(css_param->dvs_6axis);
2620			css_param->dvs_6axis = NULL;
2621
2622			dvs_6axis_config = ia_css_dvs2_6axis_config_allocate(stream);
2623			if (!dvs_6axis_config)
2624				return -ENOMEM;
2625		} else if (!dvs_6axis_config) {
2626			dvs_6axis_config = ia_css_dvs2_6axis_config_allocate(stream);
2627			if (!dvs_6axis_config)
2628				return -ENOMEM;
2629		}
2630
2631		dvs_6axis_config->exp_id = t_6axis_config.exp_id;
2632
2633		if (copy_from_compatible(dvs_6axis_config->xcoords_y,
2634					t_6axis_config.xcoords_y,
2635					t_6axis_config.width_y *
2636					t_6axis_config.height_y *
2637					sizeof(*dvs_6axis_config->xcoords_y),
2638					from_user))
2639			goto error;
2640		if (copy_from_compatible(dvs_6axis_config->ycoords_y,
2641					t_6axis_config.ycoords_y,
2642					t_6axis_config.width_y *
2643					t_6axis_config.height_y *
2644					sizeof(*dvs_6axis_config->ycoords_y),
2645					from_user))
2646			goto error;
2647		if (copy_from_compatible(dvs_6axis_config->xcoords_uv,
2648					t_6axis_config.xcoords_uv,
2649					t_6axis_config.width_uv *
2650					t_6axis_config.height_uv *
2651					sizeof(*dvs_6axis_config->xcoords_uv),
2652					from_user))
2653			goto error;
2654		if (copy_from_compatible(dvs_6axis_config->ycoords_uv,
2655					t_6axis_config.ycoords_uv,
2656					t_6axis_config.width_uv *
2657					t_6axis_config.height_uv *
2658					sizeof(*dvs_6axis_config->ycoords_uv),
2659					from_user))
2660			goto error;
2661	} else {
2662		if (old_6axis_config &&
2663		    (old_6axis_config->width_y != source_6axis_config->width_y ||
2664		    old_6axis_config->height_y != source_6axis_config->height_y ||
2665		    old_6axis_config->width_uv != source_6axis_config->width_uv ||
2666		    old_6axis_config->height_uv != source_6axis_config->height_uv)) {
2667			ia_css_dvs2_6axis_config_free(css_param->dvs_6axis);
2668			css_param->dvs_6axis = NULL;
2669
2670			dvs_6axis_config = ia_css_dvs2_6axis_config_allocate(stream);
2671			if (!dvs_6axis_config) {
2672				ret = -ENOMEM;
2673				goto error;
2674			}
2675		} else if (!dvs_6axis_config) {
2676			dvs_6axis_config = ia_css_dvs2_6axis_config_allocate(stream);
2677			if (!dvs_6axis_config) {
2678				ret = -ENOMEM;
2679				goto error;
2680			}
2681		}
2682
2683		dvs_6axis_config->exp_id = source_6axis_config->exp_id;
2684
2685		if (copy_from_compatible(dvs_6axis_config->xcoords_y,
2686					source_6axis_config->xcoords_y,
2687					source_6axis_config->width_y *
2688					source_6axis_config->height_y *
2689					sizeof(*source_6axis_config->xcoords_y),
2690					from_user))
2691			goto error;
2692		if (copy_from_compatible(dvs_6axis_config->ycoords_y,
2693					source_6axis_config->ycoords_y,
2694					source_6axis_config->width_y *
2695					source_6axis_config->height_y *
2696					sizeof(*source_6axis_config->ycoords_y),
2697					from_user))
2698			goto error;
2699		if (copy_from_compatible(dvs_6axis_config->xcoords_uv,
2700					source_6axis_config->xcoords_uv,
2701					source_6axis_config->width_uv *
2702					source_6axis_config->height_uv *
2703					sizeof(*source_6axis_config->xcoords_uv),
2704					from_user))
2705			goto error;
2706		if (copy_from_compatible(dvs_6axis_config->ycoords_uv,
2707					source_6axis_config->ycoords_uv,
2708					source_6axis_config->width_uv *
2709					source_6axis_config->height_uv *
2710					sizeof(*source_6axis_config->ycoords_uv),
2711					from_user))
2712			goto error;
2713	}
2714	css_param->dvs_6axis = dvs_6axis_config;
2715	css_param->update_flag.dvs_6axis_config =
2716	    (struct atomisp_dvs_6axis_config *)dvs_6axis_config;
2717	return 0;
2718
2719error:
2720	if (dvs_6axis_config)
2721		ia_css_dvs2_6axis_config_free(dvs_6axis_config);
2722	return ret;
2723}
2724
2725int atomisp_cp_morph_table(struct atomisp_sub_device *asd,
2726			   struct atomisp_morph_table *source_morph_table,
2727			   struct atomisp_css_params *css_param,
2728			   bool from_user)
2729{
2730	int ret = -EFAULT;
2731	unsigned int i;
2732	struct ia_css_morph_table *morph_table;
2733	struct ia_css_morph_table *old_morph_table;
2734
2735	if (!source_morph_table)
2736		return 0;
2737
2738	if (!from_user && css_param->update_flag.morph_table)
2739		return 0;
2740
2741	old_morph_table = css_param->morph_table;
2742
2743	if (IS_ISP2401) {
2744		struct ia_css_morph_table mtbl;
2745
2746		if (copy_from_compatible(&mtbl, source_morph_table,
2747				sizeof(struct atomisp_morph_table),
2748				from_user)) {
2749			dev_err(asd->isp->dev, "copy morph table failed!");
2750			return -EFAULT;
2751		}
2752
2753		morph_table = atomisp_css_morph_table_allocate(
2754				mtbl.width,
2755				mtbl.height);
2756		if (!morph_table)
2757			return -ENOMEM;
2758
2759		for (i = 0; i < IA_CSS_MORPH_TABLE_NUM_PLANES; i++) {
2760			if (copy_from_compatible(morph_table->coordinates_x[i],
2761						(__force void *)source_morph_table->coordinates_x[i],
2762						mtbl.height * mtbl.width *
2763						sizeof(*morph_table->coordinates_x[i]),
2764						from_user))
2765				goto error;
2766
2767			if (copy_from_compatible(morph_table->coordinates_y[i],
2768						(__force void *)source_morph_table->coordinates_y[i],
2769						mtbl.height * mtbl.width *
2770						sizeof(*morph_table->coordinates_y[i]),
2771						from_user))
2772				goto error;
2773		}
2774	} else {
2775		morph_table = atomisp_css_morph_table_allocate(
2776				source_morph_table->width,
2777				source_morph_table->height);
2778		if (!morph_table) {
2779			ret = -ENOMEM;
2780			goto error;
2781		}
2782
2783		for (i = 0; i < IA_CSS_MORPH_TABLE_NUM_PLANES; i++) {
2784			if (copy_from_compatible(morph_table->coordinates_x[i],
2785						(__force void *)source_morph_table->coordinates_x[i],
2786						source_morph_table->height * source_morph_table->width *
2787						sizeof(*source_morph_table->coordinates_x[i]),
2788						from_user))
2789				goto error;
2790
2791			if (copy_from_compatible(morph_table->coordinates_y[i],
2792						(__force void *)source_morph_table->coordinates_y[i],
2793						source_morph_table->height * source_morph_table->width *
2794						sizeof(*source_morph_table->coordinates_y[i]),
2795						from_user))
2796				goto error;
2797		}
2798	}
2799
2800	css_param->morph_table = morph_table;
2801	if (old_morph_table)
2802		atomisp_css_morph_table_free(old_morph_table);
2803	css_param->update_flag.morph_table =
2804	    (struct atomisp_morph_table *)morph_table;
2805	return 0;
2806
2807error:
2808	if (morph_table)
2809		atomisp_css_morph_table_free(morph_table);
2810	return ret;
2811}
2812
2813int atomisp_makeup_css_parameters(struct atomisp_sub_device *asd,
2814				  struct atomisp_parameters *arg,
2815				  struct atomisp_css_params *css_param)
2816{
2817	int ret;
2818
2819	ret = atomisp_cp_general_isp_parameters(asd, arg, css_param, false);
2820	if (ret)
2821		return ret;
2822	ret = atomisp_cp_lsc_table(asd, arg->shading_table, css_param, false);
2823	if (ret)
2824		return ret;
2825	ret = atomisp_cp_morph_table(asd, arg->morph_table, css_param, false);
2826	if (ret)
2827		return ret;
2828	ret = atomisp_css_cp_dvs2_coefs(asd,
2829					(struct ia_css_dvs2_coefficients *)arg->dvs2_coefs,
2830					css_param, false);
2831	if (ret)
2832		return ret;
2833	ret = atomisp_cp_dvs_6axis_config(asd, arg->dvs_6axis_config,
2834					  css_param, false);
2835	return ret;
2836}
2837
2838void atomisp_free_css_parameters(struct atomisp_css_params *css_param)
2839{
2840	if (css_param->dvs_6axis) {
2841		ia_css_dvs2_6axis_config_free(css_param->dvs_6axis);
2842		css_param->dvs_6axis = NULL;
2843	}
2844	if (css_param->dvs2_coeff) {
2845		ia_css_dvs2_coefficients_free(css_param->dvs2_coeff);
2846		css_param->dvs2_coeff = NULL;
2847	}
2848	if (css_param->shading_table) {
2849		ia_css_shading_table_free(css_param->shading_table);
2850		css_param->shading_table = NULL;
2851	}
2852	if (css_param->morph_table) {
2853		ia_css_morph_table_free(css_param->morph_table);
2854		css_param->morph_table = NULL;
2855	}
2856}
2857
2858static void atomisp_move_frame_to_activeq(struct ia_css_frame *frame,
2859					  struct atomisp_css_params_with_list *param)
2860{
2861	struct atomisp_video_pipe *pipe = vb_to_pipe(&frame->vb.vb2_buf);
2862	unsigned long irqflags;
2863
2864	pipe->frame_params[frame->vb.vb2_buf.index] = param;
2865	spin_lock_irqsave(&pipe->irq_lock, irqflags);
2866	list_move_tail(&frame->queue, &pipe->activeq);
2867	spin_unlock_irqrestore(&pipe->irq_lock, irqflags);
2868}
2869
2870/*
2871 * Check parameter queue list and buffer queue list to find out if matched items
2872 * and then set parameter to CSS and enqueue buffer to CSS.
2873 * Of course, if the buffer in buffer waiting list is not bound to a per-frame
2874 * parameter, it will be enqueued into CSS as long as the per-frame setting
2875 * buffers before it get enqueued.
2876 */
2877void atomisp_handle_parameter_and_buffer(struct atomisp_video_pipe *pipe)
2878{
2879	struct atomisp_sub_device *asd = pipe->asd;
2880	struct ia_css_frame *frame = NULL, *frame_tmp;
2881	struct atomisp_css_params_with_list *param = NULL, *param_tmp;
2882	bool need_to_enqueue_buffer = false;
2883	int i;
2884
2885	lockdep_assert_held(&asd->isp->mutex);
2886
2887	/*
2888	 * CSS/FW requires set parameter and enqueue buffer happen after ISP
2889	 * is streamon.
2890	 */
2891	if (!asd->streaming)
2892		return;
2893
2894	if (list_empty(&pipe->per_frame_params) ||
2895	    list_empty(&pipe->buffers_waiting_for_param))
2896		return;
2897
2898	list_for_each_entry_safe(frame, frame_tmp,
2899				 &pipe->buffers_waiting_for_param, queue) {
2900		i = frame->vb.vb2_buf.index;
2901		if (pipe->frame_request_config_id[i]) {
2902			list_for_each_entry_safe(param, param_tmp,
2903						 &pipe->per_frame_params, list) {
2904				if (pipe->frame_request_config_id[i] != param->params.isp_config_id)
2905					continue;
2906
2907				list_del(&param->list);
2908
2909				/*
2910				 * clear the request config id as the buffer
2911				 * will be handled and enqueued into CSS soon
2912				 */
2913				pipe->frame_request_config_id[i] = 0;
2914				atomisp_move_frame_to_activeq(frame, param);
2915				need_to_enqueue_buffer = true;
2916				break;
2917			}
2918
2919			/* If this is the end, stop further loop */
2920			if (list_entry_is_head(param, &pipe->per_frame_params, list))
2921				break;
2922		} else {
2923			atomisp_move_frame_to_activeq(frame, NULL);
2924			need_to_enqueue_buffer = true;
2925		}
2926	}
2927
2928	if (!need_to_enqueue_buffer)
2929		return;
2930
2931	atomisp_qbuffers_to_css(asd);
2932}
2933
2934/*
2935 * Function to configure ISP parameters
2936 */
2937int atomisp_set_parameters(struct video_device *vdev,
2938			   struct atomisp_parameters *arg)
2939{
2940	struct atomisp_video_pipe *pipe = atomisp_to_video_pipe(vdev);
2941	struct atomisp_sub_device *asd = pipe->asd;
2942	struct atomisp_css_params_with_list *param = NULL;
2943	struct atomisp_css_params *css_param = &asd->params.css_param;
2944	int ret;
2945
2946	lockdep_assert_held(&asd->isp->mutex);
2947
2948	if (!asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].stream) {
2949		dev_err(asd->isp->dev, "%s: internal error!\n", __func__);
2950		return -EINVAL;
2951	}
2952
2953	dev_dbg(asd->isp->dev, "set parameter(per_frame_setting %d) isp_config_id %d of %s\n",
2954		arg->per_frame_setting, arg->isp_config_id, vdev->name);
2955
2956	if (arg->per_frame_setting) {
2957		/*
2958		 * Per-frame setting enabled, we allocate a new parameter
2959		 * buffer to cache the parameters and only when frame buffers
2960		 * are ready, the parameters will be set to CSS.
2961		 * per-frame setting only works for the main output frame.
2962		 */
2963		param = kvzalloc(sizeof(*param), GFP_KERNEL);
2964		if (!param) {
2965			dev_err(asd->isp->dev, "%s: failed to alloc params buffer\n",
2966				__func__);
2967			return -ENOMEM;
2968		}
2969		css_param = &param->params;
2970	}
2971
2972	ret = atomisp_cp_general_isp_parameters(asd, arg, css_param, true);
2973	if (ret)
2974		goto apply_parameter_failed;
2975
2976	ret = atomisp_cp_lsc_table(asd, arg->shading_table, css_param, true);
2977	if (ret)
2978		goto apply_parameter_failed;
2979
2980	ret = atomisp_cp_morph_table(asd, arg->morph_table, css_param, true);
2981	if (ret)
2982		goto apply_parameter_failed;
2983
2984	ret = atomisp_css_cp_dvs2_coefs(asd,
2985					(struct ia_css_dvs2_coefficients *)arg->dvs2_coefs,
2986					css_param, true);
2987	if (ret)
2988		goto apply_parameter_failed;
2989
2990	ret = atomisp_cp_dvs_6axis_config(asd, arg->dvs_6axis_config,
2991					  css_param, true);
2992	if (ret)
2993		goto apply_parameter_failed;
2994
2995	if (!arg->per_frame_setting) {
2996		/* indicate to CSS that we have parameters to be updated */
2997		asd->params.css_update_params_needed = true;
2998	} else {
2999		list_add_tail(&param->list, &pipe->per_frame_params);
3000		atomisp_handle_parameter_and_buffer(pipe);
3001	}
3002
3003	return 0;
3004
3005apply_parameter_failed:
3006	if (css_param)
3007		atomisp_free_css_parameters(css_param);
3008	kvfree(param);
3009
3010	return ret;
3011}
3012
3013/*
3014 * Function to set/get isp parameters to isp
3015 */
3016int atomisp_param(struct atomisp_sub_device *asd, int flag,
3017		  struct atomisp_parm *config)
3018{
3019	struct ia_css_pipe_config *vp_cfg =
3020		    &asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].
3021		    pipe_configs[IA_CSS_PIPE_ID_VIDEO];
3022
3023	/* Read parameter for 3A binary info */
3024	if (flag == 0) {
3025		struct ia_css_dvs_grid_info *dvs_grid_info =
3026		    atomisp_css_get_dvs_grid_info(
3027			&asd->params.curr_grid_info);
3028
3029		atomisp_curr_user_grid_info(asd, &config->info);
3030
3031		/* We always return the resolution and stride even if there is
3032		 * no valid metadata. This allows the caller to get the
3033		 * information needed to allocate user-space buffers. */
3034		config->metadata_config.metadata_height = asd->
3035			stream_env[ATOMISP_INPUT_STREAM_GENERAL].stream_info.
3036			metadata_info.resolution.height;
3037		config->metadata_config.metadata_stride = asd->
3038			stream_env[ATOMISP_INPUT_STREAM_GENERAL].stream_info.
3039			metadata_info.stride;
3040
3041		/* update dvs grid info */
3042		if (dvs_grid_info)
3043			memcpy(&config->dvs_grid,
3044			       dvs_grid_info,
3045			       sizeof(struct ia_css_dvs_grid_info));
3046
3047		if (asd->run_mode->val != ATOMISP_RUN_MODE_VIDEO) {
3048			config->dvs_envelop.width = 0;
3049			config->dvs_envelop.height = 0;
3050			return 0;
3051		}
3052
3053		/* update dvs envelop info */
3054		config->dvs_envelop.width = vp_cfg->dvs_envelope.width;
3055		config->dvs_envelop.height = vp_cfg->dvs_envelope.height;
3056		return 0;
3057	}
3058
3059	memcpy(&asd->params.css_param.wb_config, &config->wb_config,
3060	       sizeof(struct ia_css_wb_config));
3061	memcpy(&asd->params.css_param.ob_config, &config->ob_config,
3062	       sizeof(struct ia_css_ob_config));
3063	memcpy(&asd->params.css_param.dp_config, &config->dp_config,
3064	       sizeof(struct ia_css_dp_config));
3065	memcpy(&asd->params.css_param.de_config, &config->de_config,
3066	       sizeof(struct ia_css_de_config));
3067	memcpy(&asd->params.css_param.dz_config, &config->dz_config,
3068	       sizeof(struct ia_css_dz_config));
3069	memcpy(&asd->params.css_param.ce_config, &config->ce_config,
3070	       sizeof(struct ia_css_ce_config));
3071	memcpy(&asd->params.css_param.nr_config, &config->nr_config,
3072	       sizeof(struct ia_css_nr_config));
3073	memcpy(&asd->params.css_param.ee_config, &config->ee_config,
3074	       sizeof(struct ia_css_ee_config));
3075	memcpy(&asd->params.css_param.tnr_config, &config->tnr_config,
3076	       sizeof(struct ia_css_tnr_config));
3077
3078	if (asd->params.color_effect == V4L2_COLORFX_NEGATIVE) {
3079		asd->params.css_param.cc_config.matrix[3] = -config->cc_config.matrix[3];
3080		asd->params.css_param.cc_config.matrix[4] = -config->cc_config.matrix[4];
3081		asd->params.css_param.cc_config.matrix[5] = -config->cc_config.matrix[5];
3082		asd->params.css_param.cc_config.matrix[6] = -config->cc_config.matrix[6];
3083		asd->params.css_param.cc_config.matrix[7] = -config->cc_config.matrix[7];
3084		asd->params.css_param.cc_config.matrix[8] = -config->cc_config.matrix[8];
3085	}
3086
3087	if (asd->params.color_effect != V4L2_COLORFX_SEPIA &&
3088	    asd->params.color_effect != V4L2_COLORFX_BW) {
3089		memcpy(&asd->params.css_param.cc_config, &config->cc_config,
3090		       sizeof(struct ia_css_cc_config));
3091		asd->params.config.cc_config = &asd->params.css_param.cc_config;
3092	}
3093
3094	asd->params.config.wb_config = &asd->params.css_param.wb_config;
3095	asd->params.config.ob_config = &asd->params.css_param.ob_config;
3096	asd->params.config.de_config = &asd->params.css_param.de_config;
3097	asd->params.config.dz_config = &asd->params.css_param.dz_config;
3098	asd->params.config.ce_config = &asd->params.css_param.ce_config;
3099	asd->params.config.dp_config = &asd->params.css_param.dp_config;
3100	asd->params.config.nr_config = &asd->params.css_param.nr_config;
3101	asd->params.config.ee_config = &asd->params.css_param.ee_config;
3102	asd->params.config.tnr_config = &asd->params.css_param.tnr_config;
3103	asd->params.css_update_params_needed = true;
3104
3105	return 0;
3106}
3107
3108/*
3109 * Function to configure color effect of the image
3110 */
3111int atomisp_color_effect(struct atomisp_sub_device *asd, int flag,
3112			 __s32 *effect)
3113{
3114	struct ia_css_cc_config *cc_config = NULL;
3115	struct ia_css_macc_table *macc_table = NULL;
3116	struct ia_css_ctc_table *ctc_table = NULL;
3117	int ret = 0;
3118	struct v4l2_control control;
3119	struct atomisp_device *isp = asd->isp;
3120
3121	if (flag == 0) {
3122		*effect = asd->params.color_effect;
3123		return 0;
3124	}
3125
3126	control.id = V4L2_CID_COLORFX;
3127	control.value = *effect;
3128	ret =
3129	    v4l2_s_ctrl(NULL, isp->inputs[asd->input_curr].sensor->ctrl_handler,
3130			&control);
3131	/*
3132	 * if set color effect to sensor successfully, return
3133	 * 0 directly.
3134	 */
3135	if (!ret) {
3136		asd->params.color_effect = (u32)*effect;
3137		return 0;
3138	}
3139
3140	if (*effect == asd->params.color_effect)
3141		return 0;
3142
3143	/*
3144	 * isp_subdev->params.macc_en should be set to false.
3145	 */
3146	asd->params.macc_en = false;
3147
3148	switch (*effect) {
3149	case V4L2_COLORFX_NONE:
3150		macc_table = &asd->params.css_param.macc_table;
3151		asd->params.macc_en = true;
3152		break;
3153	case V4L2_COLORFX_SEPIA:
3154		cc_config = &sepia_cc_config;
3155		break;
3156	case V4L2_COLORFX_NEGATIVE:
3157		cc_config = &nega_cc_config;
3158		break;
3159	case V4L2_COLORFX_BW:
3160		cc_config = &mono_cc_config;
3161		break;
3162	case V4L2_COLORFX_SKY_BLUE:
3163		macc_table = &blue_macc_table;
3164		asd->params.macc_en = true;
3165		break;
3166	case V4L2_COLORFX_GRASS_GREEN:
3167		macc_table = &green_macc_table;
3168		asd->params.macc_en = true;
3169		break;
3170	case V4L2_COLORFX_SKIN_WHITEN_LOW:
3171		macc_table = &skin_low_macc_table;
3172		asd->params.macc_en = true;
3173		break;
3174	case V4L2_COLORFX_SKIN_WHITEN:
3175		macc_table = &skin_medium_macc_table;
3176		asd->params.macc_en = true;
3177		break;
3178	case V4L2_COLORFX_SKIN_WHITEN_HIGH:
3179		macc_table = &skin_high_macc_table;
3180		asd->params.macc_en = true;
3181		break;
3182	case V4L2_COLORFX_VIVID:
3183		ctc_table = &vivid_ctc_table;
3184		break;
3185	default:
3186		return -EINVAL;
3187	}
3188	atomisp_update_capture_mode(asd);
3189
3190	if (cc_config)
3191		asd->params.config.cc_config = cc_config;
3192	if (macc_table)
3193		asd->params.config.macc_table = macc_table;
3194	if (ctc_table)
3195		atomisp_css_set_ctc_table(asd, ctc_table);
3196	asd->params.color_effect = (u32)*effect;
3197	asd->params.css_update_params_needed = true;
3198	return 0;
3199}
3200
3201/*
3202 * Function to configure bad pixel correction
3203 */
3204int atomisp_bad_pixel(struct atomisp_sub_device *asd, int flag,
3205		      __s32 *value)
3206{
3207	if (flag == 0) {
3208		*value = asd->params.bad_pixel_en;
3209		return 0;
3210	}
3211	asd->params.bad_pixel_en = !!*value;
3212
3213	return 0;
3214}
3215
3216/*
3217 * Function to configure bad pixel correction params
3218 */
3219int atomisp_bad_pixel_param(struct atomisp_sub_device *asd, int flag,
3220			    struct atomisp_dp_config *config)
3221{
3222	if (flag == 0) {
3223		/* Get bad pixel from current setup */
3224		if (atomisp_css_get_dp_config(asd, config))
3225			return -EINVAL;
3226	} else {
3227		/* Set bad pixel to isp parameters */
3228		memcpy(&asd->params.css_param.dp_config, config,
3229		       sizeof(asd->params.css_param.dp_config));
3230		asd->params.config.dp_config = &asd->params.css_param.dp_config;
3231		asd->params.css_update_params_needed = true;
3232	}
3233
3234	return 0;
3235}
3236
3237/*
3238 * Function to enable/disable video image stablization
3239 */
3240int atomisp_video_stable(struct atomisp_sub_device *asd, int flag,
3241			 __s32 *value)
3242{
3243	if (flag == 0)
3244		*value = asd->params.video_dis_en;
3245	else
3246		asd->params.video_dis_en = !!*value;
3247
3248	return 0;
3249}
3250
3251/*
3252 * Function to configure fixed pattern noise
3253 */
3254int atomisp_fixed_pattern(struct atomisp_sub_device *asd, int flag,
3255			  __s32 *value)
3256{
3257	if (flag == 0) {
3258		*value = asd->params.fpn_en;
3259		return 0;
3260	}
3261
3262	if (*value == 0) {
3263		asd->params.fpn_en = false;
3264		return 0;
3265	}
3266
3267	/* Add function to get black from sensor with shutter off */
3268	return 0;
3269}
3270
3271static unsigned int
3272atomisp_bytesperline_to_padded_width(unsigned int bytesperline,
3273				     enum ia_css_frame_format format)
3274{
3275	switch (format) {
3276	case IA_CSS_FRAME_FORMAT_UYVY:
3277	case IA_CSS_FRAME_FORMAT_YUYV:
3278	case IA_CSS_FRAME_FORMAT_RAW:
3279	case IA_CSS_FRAME_FORMAT_RGB565:
3280		return bytesperline / 2;
3281	case IA_CSS_FRAME_FORMAT_RGBA888:
3282		return bytesperline / 4;
3283	/* The following cases could be removed, but we leave them
3284	   in to document the formats that are included. */
3285	case IA_CSS_FRAME_FORMAT_NV11:
3286	case IA_CSS_FRAME_FORMAT_NV12:
3287	case IA_CSS_FRAME_FORMAT_NV16:
3288	case IA_CSS_FRAME_FORMAT_NV21:
3289	case IA_CSS_FRAME_FORMAT_NV61:
3290	case IA_CSS_FRAME_FORMAT_YV12:
3291	case IA_CSS_FRAME_FORMAT_YV16:
3292	case IA_CSS_FRAME_FORMAT_YUV420:
3293	case IA_CSS_FRAME_FORMAT_YUV420_16:
3294	case IA_CSS_FRAME_FORMAT_YUV422:
3295	case IA_CSS_FRAME_FORMAT_YUV422_16:
3296	case IA_CSS_FRAME_FORMAT_YUV444:
3297	case IA_CSS_FRAME_FORMAT_YUV_LINE:
3298	case IA_CSS_FRAME_FORMAT_PLANAR_RGB888:
3299	case IA_CSS_FRAME_FORMAT_QPLANE6:
3300	case IA_CSS_FRAME_FORMAT_BINARY_8:
3301	default:
3302		return bytesperline;
3303	}
3304}
3305
3306static int
3307atomisp_v4l2_framebuffer_to_css_frame(const struct v4l2_framebuffer *arg,
3308				      struct ia_css_frame **result)
3309{
3310	struct ia_css_frame *res = NULL;
3311	unsigned int padded_width;
3312	enum ia_css_frame_format sh_format;
3313	char *tmp_buf = NULL;
3314	int ret = 0;
3315
3316	sh_format = v4l2_fmt_to_sh_fmt(arg->fmt.pixelformat);
3317	padded_width = atomisp_bytesperline_to_padded_width(
3318			   arg->fmt.bytesperline, sh_format);
3319
3320	/* Note: the padded width on an ia_css_frame is in elements, not in
3321	   bytes. The RAW frame we use here should always be a 16bit RAW
3322	   frame. This is why we bytesperline/2 is equal to the padded with */
3323	if (ia_css_frame_allocate(&res, arg->fmt.width, arg->fmt.height,
3324				       sh_format, padded_width, 0)) {
3325		ret = -ENOMEM;
3326		goto err;
3327	}
3328
3329	tmp_buf = vmalloc(arg->fmt.sizeimage);
3330	if (!tmp_buf) {
3331		ret = -ENOMEM;
3332		goto err;
3333	}
3334	if (copy_from_user(tmp_buf, (void __user __force *)arg->base,
3335			   arg->fmt.sizeimage)) {
3336		ret = -EFAULT;
3337		goto err;
3338	}
3339
3340	if (hmm_store(res->data, tmp_buf, arg->fmt.sizeimage)) {
3341		ret = -EINVAL;
3342		goto err;
3343	}
3344
3345err:
3346	if (ret && res)
3347		ia_css_frame_free(res);
3348	vfree(tmp_buf);
3349	if (ret == 0)
3350		*result = res;
3351	return ret;
3352}
3353
3354/*
3355 * Function to configure fixed pattern noise table
3356 */
3357int atomisp_fixed_pattern_table(struct atomisp_sub_device *asd,
3358				struct v4l2_framebuffer *arg)
3359{
3360	struct ia_css_frame *raw_black_frame = NULL;
3361	int ret;
3362
3363	if (!arg)
3364		return -EINVAL;
3365
3366	ret = atomisp_v4l2_framebuffer_to_css_frame(arg, &raw_black_frame);
3367	if (ret)
3368		return ret;
3369
3370	if (sh_css_set_black_frame(asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].stream,
3371				   raw_black_frame) != 0)
3372		return -ENOMEM;
3373
3374	ia_css_frame_free(raw_black_frame);
3375	return ret;
3376}
3377
3378/*
3379 * Function to configure false color correction
3380 */
3381int atomisp_false_color(struct atomisp_sub_device *asd, int flag,
3382			__s32 *value)
3383{
3384	/* Get nr config from current setup */
3385	if (flag == 0) {
3386		*value = asd->params.false_color;
3387		return 0;
3388	}
3389
3390	/* Set nr config to isp parameters */
3391	if (*value) {
3392		asd->params.config.de_config = NULL;
3393	} else {
3394		asd->params.css_param.de_config.pixelnoise = 0;
3395		asd->params.config.de_config = &asd->params.css_param.de_config;
3396	}
3397	asd->params.css_update_params_needed = true;
3398	asd->params.false_color = *value;
3399	return 0;
3400}
3401
3402/*
3403 * Function to configure bad pixel correction params
3404 */
3405int atomisp_false_color_param(struct atomisp_sub_device *asd, int flag,
3406			      struct atomisp_de_config *config)
3407{
3408	if (flag == 0) {
3409		/* Get false color from current setup */
3410		if (atomisp_css_get_de_config(asd, config))
3411			return -EINVAL;
3412	} else {
3413		/* Set false color to isp parameters */
3414		memcpy(&asd->params.css_param.de_config, config,
3415		       sizeof(asd->params.css_param.de_config));
3416		asd->params.config.de_config = &asd->params.css_param.de_config;
3417		asd->params.css_update_params_needed = true;
3418	}
3419
3420	return 0;
3421}
3422
3423/*
3424 * Function to configure white balance params
3425 */
3426int atomisp_white_balance_param(struct atomisp_sub_device *asd, int flag,
3427				struct atomisp_wb_config *config)
3428{
3429	if (flag == 0) {
3430		/* Get white balance from current setup */
3431		if (atomisp_css_get_wb_config(asd, config))
3432			return -EINVAL;
3433	} else {
3434		/* Set white balance to isp parameters */
3435		memcpy(&asd->params.css_param.wb_config, config,
3436		       sizeof(asd->params.css_param.wb_config));
3437		asd->params.config.wb_config = &asd->params.css_param.wb_config;
3438		asd->params.css_update_params_needed = true;
3439	}
3440
3441	return 0;
3442}
3443
3444int atomisp_3a_config_param(struct atomisp_sub_device *asd, int flag,
3445			    struct atomisp_3a_config *config)
3446{
3447	struct atomisp_device *isp = asd->isp;
3448
3449	dev_dbg(isp->dev, ">%s %d\n", __func__, flag);
3450
3451	if (flag == 0) {
3452		/* Get white balance from current setup */
3453		if (atomisp_css_get_3a_config(asd, config))
3454			return -EINVAL;
3455	} else {
3456		/* Set white balance to isp parameters */
3457		memcpy(&asd->params.css_param.s3a_config, config,
3458		       sizeof(asd->params.css_param.s3a_config));
3459		asd->params.config.s3a_config = &asd->params.css_param.s3a_config;
3460		asd->params.css_update_params_needed = true;
3461	}
3462
3463	dev_dbg(isp->dev, "<%s %d\n", __func__, flag);
3464	return 0;
3465}
3466
3467/*
3468 * Function to setup digital zoom
3469 */
3470int atomisp_digital_zoom(struct atomisp_sub_device *asd, int flag,
3471			 __s32 *value)
3472{
3473	u32 zoom;
3474	struct atomisp_device *isp = asd->isp;
3475
3476	unsigned int max_zoom = MRFLD_MAX_ZOOM_FACTOR;
3477
3478	if (flag == 0) {
3479		atomisp_css_get_zoom_factor(asd, &zoom);
3480		*value = max_zoom - zoom;
3481	} else {
3482		if (*value < 0)
3483			return -EINVAL;
3484
3485		zoom = max_zoom - min_t(u32, max_zoom - 1, *value);
3486		atomisp_css_set_zoom_factor(asd, zoom);
3487
3488		dev_dbg(isp->dev, "%s, zoom: %d\n", __func__, zoom);
3489		asd->params.css_update_params_needed = true;
3490	}
3491
3492	return 0;
3493}
3494
3495static void __atomisp_update_stream_env(struct atomisp_sub_device *asd,
3496					u16 stream_index, struct atomisp_input_stream_info *stream_info)
3497{
3498	int i;
3499
3500	/* assign virtual channel id return from sensor driver query */
3501	asd->stream_env[stream_index].ch_id = stream_info->ch_id;
3502	asd->stream_env[stream_index].isys_configs = stream_info->isys_configs;
3503	for (i = 0; i < stream_info->isys_configs; i++) {
3504		asd->stream_env[stream_index].isys_info[i].input_format =
3505		    stream_info->isys_info[i].input_format;
3506		asd->stream_env[stream_index].isys_info[i].width =
3507		    stream_info->isys_info[i].width;
3508		asd->stream_env[stream_index].isys_info[i].height =
3509		    stream_info->isys_info[i].height;
3510	}
3511}
3512
3513static void __atomisp_init_stream_info(u16 stream_index,
3514				       struct atomisp_input_stream_info *stream_info)
3515{
3516	int i;
3517
3518	stream_info->enable = 1;
3519	stream_info->stream = stream_index;
3520	stream_info->ch_id = 0;
3521	stream_info->isys_configs = 0;
3522	for (i = 0; i < MAX_STREAMS_PER_CHANNEL; i++) {
3523		stream_info->isys_info[i].input_format = 0;
3524		stream_info->isys_info[i].width = 0;
3525		stream_info->isys_info[i].height = 0;
3526	}
3527}
3528
3529static void atomisp_fill_pix_format(struct v4l2_pix_format *f,
3530				    u32 width, u32 height,
3531				    const struct atomisp_format_bridge *br_fmt)
3532{
3533	u32 bytes;
3534
3535	f->width = width;
3536	f->height = height;
3537	f->pixelformat = br_fmt->pixelformat;
3538
3539	/* Adding padding to width for bytesperline calculation */
3540	width = ia_css_frame_pad_width(width, br_fmt->sh_fmt);
3541	bytes = BITS_TO_BYTES(br_fmt->depth * width);
3542
3543	if (br_fmt->planar)
3544		f->bytesperline = width;
3545	else
3546		f->bytesperline = bytes;
3547
3548	f->sizeimage = PAGE_ALIGN(height * bytes);
3549
3550	if (f->field == V4L2_FIELD_ANY)
3551		f->field = V4L2_FIELD_NONE;
3552
3553	/*
3554	 * FIXME: do we need to set this up differently, depending on the
3555	 * sensor or the pipeline?
3556	 */
3557	f->colorspace = V4L2_COLORSPACE_REC709;
3558	f->ycbcr_enc = V4L2_YCBCR_ENC_709;
3559	f->xfer_func = V4L2_XFER_FUNC_709;
3560}
3561
3562/* Get sensor padding values for the non padded width x height resolution */
3563void atomisp_get_padding(struct atomisp_device *isp, u32 width, u32 height,
3564			 u32 *padding_w, u32 *padding_h)
3565{
3566	struct atomisp_input_subdev *input = &isp->inputs[isp->asd.input_curr];
3567	struct v4l2_rect native_rect = input->native_rect;
3568	const struct atomisp_in_fmt_conv *fc = NULL;
3569	u32 min_pad_w = ISP2400_MIN_PAD_W;
3570	u32 min_pad_h = ISP2400_MIN_PAD_H;
3571	struct v4l2_mbus_framefmt *sink;
3572
3573	if (!input->crop_support) {
3574		*padding_w = pad_w;
3575		*padding_h = pad_h;
3576		return;
3577	}
3578
3579	width = min(width, input->active_rect.width);
3580	height = min(height, input->active_rect.height);
3581
3582	if (input->binning_support && width <= (input->active_rect.width / 2) &&
3583				      height <= (input->active_rect.height / 2)) {
3584		native_rect.width /= 2;
3585		native_rect.height /= 2;
3586	}
3587
3588	*padding_w = min_t(u32, (native_rect.width - width) & ~1, pad_w);
3589	*padding_h = min_t(u32, (native_rect.height - height) & ~1, pad_h);
3590
3591	/* The below minimum padding requirements are for BYT / ISP2400 only */
3592	if (IS_ISP2401)
3593		return;
3594
3595	sink = atomisp_subdev_get_ffmt(&isp->asd.subdev, NULL, V4L2_SUBDEV_FORMAT_ACTIVE,
3596				       ATOMISP_SUBDEV_PAD_SINK);
3597	if (sink)
3598		fc = atomisp_find_in_fmt_conv(sink->code);
3599	if (!fc) {
3600		dev_warn(isp->dev, "%s: Could not get sensor format\n", __func__);
3601		goto apply_min_padding;
3602	}
3603
3604	/*
3605	 * The ISP only supports GRBG for other bayer-orders additional padding
3606	 * is used so that the raw sensor data can be cropped to fix the order.
3607	 */
3608	if (fc->bayer_order == IA_CSS_BAYER_ORDER_RGGB ||
3609	    fc->bayer_order == IA_CSS_BAYER_ORDER_GBRG)
3610		min_pad_w += 2;
3611
3612	if (fc->bayer_order == IA_CSS_BAYER_ORDER_BGGR ||
3613	    fc->bayer_order == IA_CSS_BAYER_ORDER_GBRG)
3614		min_pad_h += 2;
3615
3616apply_min_padding:
3617	*padding_w = max_t(u32, *padding_w, min_pad_w);
3618	*padding_h = max_t(u32, *padding_h, min_pad_h);
3619}
3620
3621int atomisp_s_sensor_power(struct atomisp_device *isp, unsigned int input, bool on)
3622{
3623	int ret;
3624
3625	if (isp->inputs[input].sensor_on == on)
3626		return 0;
3627
3628	ret = v4l2_subdev_call(isp->inputs[input].sensor, core, s_power, on);
3629	if (ret && ret != -ENOIOCTLCMD) {
3630		dev_err(isp->dev, "Error setting sensor power %d: %d\n", on, ret);
3631		return ret;
3632	}
3633
3634	isp->inputs[input].sensor_on = on;
3635	return 0;
3636}
3637
3638int atomisp_select_input(struct atomisp_device *isp, unsigned int input)
3639{
3640	unsigned int input_orig = isp->asd.input_curr;
3641	int ret;
3642
3643	/* Power on new sensor */
3644	ret = atomisp_s_sensor_power(isp, input, 1);
3645	if (ret)
3646		return ret;
3647
3648	isp->asd.input_curr = input;
3649
3650	/* Power off previous sensor */
3651	if (input != input_orig)
3652		atomisp_s_sensor_power(isp, input_orig, 0);
3653
3654	atomisp_setup_input_links(isp);
3655	return 0;
3656}
3657
3658/*
3659 * Ensure the CSI-receiver -> ISP link for input_curr is marked as enabled and
3660 * the other CSI-receiver -> ISP links are disabled.
3661 */
3662void atomisp_setup_input_links(struct atomisp_device *isp)
3663{
3664	struct media_link *link;
3665
3666	lockdep_assert_held(&isp->media_dev.graph_mutex);
3667
3668	for (int i = 0; i < ATOMISP_CAMERA_NR_PORTS; i++) {
3669		link = media_entity_find_link(
3670				&isp->csi2_port[i].subdev.entity.pads[CSI2_PAD_SOURCE],
3671				&isp->asd.subdev.entity.pads[ATOMISP_SUBDEV_PAD_SINK]);
3672		if (!link) {
3673			dev_err(isp->dev, "Error cannot find CSI2-port[%d] -> ISP link\n", i);
3674			continue; /* Should never happen */
3675		}
3676
3677		/*
3678		 * Modify the flags directly, calling media_entity_setup_link()
3679		 * will end up calling atomisp_link_setup() which calls this
3680		 * function again leading to endless recursion.
3681		 */
3682		if (isp->sensor_subdevs[i] == isp->inputs[isp->asd.input_curr].csi_remote_source)
3683			link->flags |= MEDIA_LNK_FL_ENABLED;
3684		else
3685			link->flags &= ~MEDIA_LNK_FL_ENABLED;
3686
3687		link->reverse->flags = link->flags;
3688	}
3689}
3690
3691static int atomisp_set_sensor_crop_and_fmt(struct atomisp_device *isp,
3692					   struct v4l2_mbus_framefmt *ffmt,
3693					   int which)
3694{
3695	struct atomisp_input_subdev *input = &isp->inputs[isp->asd.input_curr];
3696	struct v4l2_subdev_selection sel = {
3697		.which = which,
3698		.target = V4L2_SEL_TGT_CROP,
3699		.r.width = ffmt->width,
3700		.r.height = ffmt->height,
3701	};
3702	struct v4l2_subdev_format format = {
3703		.which = which,
3704		.format = *ffmt,
3705	};
3706	struct v4l2_subdev_state *sd_state;
3707	int ret = 0;
3708
3709	if (!input->sensor)
3710		return -EINVAL;
3711
3712	/*
3713	 * Some old sensor drivers already write the registers on set_fmt
3714	 * instead of on stream on, power on the sensor now (on newer
3715	 * sensor drivers the s_power op is a no-op).
3716	 */
3717	if (which == V4L2_SUBDEV_FORMAT_ACTIVE) {
3718		ret = atomisp_s_sensor_power(isp, isp->asd.input_curr, 1);
3719		if (ret)
3720			return ret;
3721	}
3722
3723	sd_state = (which == V4L2_SUBDEV_FORMAT_TRY) ? input->try_sd_state :
3724						       input->sensor->active_state;
3725	if (sd_state)
3726		v4l2_subdev_lock_state(sd_state);
3727
3728	if (!input->crop_support)
3729		goto set_fmt;
3730
3731	/* Cropping is done before binning, when binning double the crop rect */
3732	if (input->binning_support && sel.r.width <= (input->native_rect.width / 2) &&
3733				      sel.r.height <= (input->native_rect.height / 2)) {
3734		sel.r.width *= 2;
3735		sel.r.height *= 2;
3736	}
3737
3738	/* Clamp to avoid top/left calculations overflowing */
3739	sel.r.width = min(sel.r.width, input->native_rect.width);
3740	sel.r.height = min(sel.r.height, input->native_rect.height);
3741
3742	sel.r.left = ((input->native_rect.width - sel.r.width) / 2) & ~1;
3743	sel.r.top = ((input->native_rect.height - sel.r.height) / 2) & ~1;
3744
3745	ret = v4l2_subdev_call(input->sensor, pad, set_selection, sd_state, &sel);
3746	if (ret)
3747		dev_err(isp->dev, "Error setting crop to (%d,%d)/%ux%u: %d\n",
3748			sel.r.left, sel.r.top, sel.r.width, sel.r.height, ret);
3749
3750set_fmt:
3751	if (ret == 0) {
3752		ret = v4l2_subdev_call(input->sensor, pad, set_fmt, sd_state, &format);
3753		dev_dbg(isp->dev, "Set sensor format ret: %d size %dx%d\n",
3754			ret, format.format.width, format.format.height);
3755	}
3756
3757	if (sd_state)
3758		v4l2_subdev_unlock_state(sd_state);
3759
3760	/* Propagate new fmt to sensor ISP */
3761	if (ret == 0 && which == V4L2_SUBDEV_FORMAT_ACTIVE && input->sensor_isp) {
3762		sd_state = v4l2_subdev_lock_and_get_active_state(input->sensor_isp);
3763
3764		format.pad = SENSOR_ISP_PAD_SINK;
3765		ret = v4l2_subdev_call(input->sensor_isp, pad, set_fmt, sd_state, &format);
3766		dev_dbg(isp->dev, "Set sensor ISP sink format ret: %d size %dx%d\n",
3767			ret, format.format.width, format.format.height);
3768
3769		if (ret == 0) {
3770			format.pad = SENSOR_ISP_PAD_SOURCE;
3771			ret = v4l2_subdev_call(input->sensor_isp, pad, set_fmt, sd_state, &format);
3772			dev_dbg(isp->dev, "Set sensor ISP source format ret: %d size %dx%d\n",
3773				ret, format.format.width, format.format.height);
3774		}
3775
3776		if (sd_state)
3777			v4l2_subdev_unlock_state(sd_state);
3778	}
3779
3780	/* Propagate new fmt to CSI port */
3781	if (ret == 0 && which == V4L2_SUBDEV_FORMAT_ACTIVE) {
3782		format.pad = CSI2_PAD_SINK;
3783		ret = v4l2_subdev_call(input->csi_port, pad, set_fmt, NULL, &format);
3784		if (ret)
3785			return ret;
3786	}
3787
3788	*ffmt = format.format;
3789	return ret;
3790}
3791
3792/* This function looks up the closest available resolution. */
3793int atomisp_try_fmt(struct atomisp_device *isp, struct v4l2_pix_format *f,
3794		    const struct atomisp_format_bridge **fmt_ret,
3795		    const struct atomisp_format_bridge **snr_fmt_ret)
3796{
3797	const struct atomisp_format_bridge *fmt, *snr_fmt;
3798	struct atomisp_sub_device *asd = &isp->asd;
3799	struct v4l2_mbus_framefmt ffmt = { };
3800	u32 padding_w, padding_h;
3801	int ret;
3802
3803	fmt = atomisp_get_format_bridge(f->pixelformat);
3804	/* Currently, raw formats are broken!!! */
3805	if (!fmt || fmt->sh_fmt == IA_CSS_FRAME_FORMAT_RAW) {
3806		f->pixelformat = V4L2_PIX_FMT_YUV420;
3807
3808		fmt = atomisp_get_format_bridge(f->pixelformat);
3809		if (!fmt)
3810			return -EINVAL;
3811	}
3812
3813	/*
3814	 * The preview pipeline does not support width > 1920. Also limit height
3815	 * to avoid sensor drivers still picking a too wide resolution.
3816	 */
3817	if (asd->run_mode->val == ATOMISP_RUN_MODE_PREVIEW) {
3818		f->width = min(f->width, 1920U);
3819		f->height = min(f->height, 1440U);
3820	}
3821
3822	/*
3823	 * atomisp_set_fmt() will set the sensor resolution to the requested
3824	 * resolution + padding. Add padding here and remove it again after
3825	 * the set_fmt call, like atomisp_set_fmt_to_snr() does.
3826	 */
3827	atomisp_get_padding(isp, f->width, f->height, &padding_w, &padding_h);
3828	v4l2_fill_mbus_format(&ffmt, f, fmt->mbus_code);
3829	ffmt.width += padding_w;
3830	ffmt.height += padding_h;
3831
3832	dev_dbg(isp->dev, "try_mbus_fmt: try %ux%u\n", ffmt.width, ffmt.height);
3833
3834	ret = atomisp_set_sensor_crop_and_fmt(isp, &ffmt, V4L2_SUBDEV_FORMAT_TRY);
3835	if (ret)
3836		return ret;
3837
3838	dev_dbg(isp->dev, "try_mbus_fmt: got %ux%u\n", ffmt.width, ffmt.height);
3839
3840	snr_fmt = atomisp_get_format_bridge_from_mbus(ffmt.code);
3841	if (!snr_fmt) {
3842		dev_err(isp->dev, "unknown sensor format 0x%8.8x\n",
3843			ffmt.code);
3844		return -EINVAL;
3845	}
3846
3847	f->width = ffmt.width - padding_w;
3848	f->height = ffmt.height - padding_h;
3849
3850	/*
3851	 * If the format is jpeg or custom RAW, then the width and height will
3852	 * not satisfy the normal atomisp requirements and no need to check
3853	 * the below conditions. So just assign to what is being returned from
3854	 * the sensor driver.
3855	 */
3856	if (f->pixelformat == V4L2_PIX_FMT_JPEG ||
3857	    f->pixelformat == V4L2_PIX_FMT_CUSTOM_M10MO_RAW)
3858		goto out_fill_pix_format;
3859
3860	/* app vs isp */
3861	f->width = rounddown(clamp_t(u32, f->width, ATOM_ISP_MIN_WIDTH,
3862				     ATOM_ISP_MAX_WIDTH), ATOM_ISP_STEP_WIDTH);
3863	f->height = rounddown(clamp_t(u32, f->height, ATOM_ISP_MIN_HEIGHT,
3864				      ATOM_ISP_MAX_HEIGHT), ATOM_ISP_STEP_HEIGHT);
3865
3866out_fill_pix_format:
3867	atomisp_fill_pix_format(f, f->width, f->height, fmt);
3868
3869	if (fmt_ret)
3870		*fmt_ret = fmt;
3871
3872	if (snr_fmt_ret)
3873		*snr_fmt_ret = snr_fmt;
3874
3875	return 0;
3876}
3877
3878enum mipi_port_id atomisp_port_to_mipi_port(struct atomisp_device *isp,
3879					    enum atomisp_camera_port port)
3880{
3881	switch (port) {
3882	case ATOMISP_CAMERA_PORT_PRIMARY:
3883		return MIPI_PORT0_ID;
3884	case ATOMISP_CAMERA_PORT_SECONDARY:
3885		return MIPI_PORT1_ID;
3886	case ATOMISP_CAMERA_PORT_TERTIARY:
3887		return MIPI_PORT2_ID;
3888	default:
3889		dev_err(isp->dev, "unsupported port: %d\n", port);
3890		return MIPI_PORT0_ID;
3891	}
3892}
3893
3894static inline int atomisp_set_sensor_mipi_to_isp(
3895    struct atomisp_sub_device *asd,
3896    enum atomisp_input_stream_id stream_id,
3897    struct camera_mipi_info *mipi_info)
3898{
3899	struct v4l2_control ctrl;
3900	struct atomisp_device *isp = asd->isp;
3901	struct atomisp_input_subdev *input = &isp->inputs[asd->input_curr];
3902	const struct atomisp_in_fmt_conv *fc;
3903	int mipi_freq = 0;
3904	unsigned int input_format, bayer_order;
3905	enum atomisp_input_format metadata_format = ATOMISP_INPUT_FORMAT_EMBEDDED;
3906	u32 mipi_port, metadata_width = 0, metadata_height = 0;
3907
3908	ctrl.id = V4L2_CID_LINK_FREQ;
3909	if (v4l2_g_ctrl(input->sensor->ctrl_handler, &ctrl) == 0)
3910		mipi_freq = ctrl.value;
3911
3912	if (asd->stream_env[stream_id].isys_configs == 1) {
3913		input_format =
3914		    asd->stream_env[stream_id].isys_info[0].input_format;
3915		atomisp_css_isys_set_format(asd, stream_id,
3916					    input_format, IA_CSS_STREAM_DEFAULT_ISYS_STREAM_IDX);
3917	} else if (asd->stream_env[stream_id].isys_configs == 2) {
3918		atomisp_css_isys_two_stream_cfg_update_stream1(
3919		    asd, stream_id,
3920		    asd->stream_env[stream_id].isys_info[0].input_format,
3921		    asd->stream_env[stream_id].isys_info[0].width,
3922		    asd->stream_env[stream_id].isys_info[0].height);
3923
3924		atomisp_css_isys_two_stream_cfg_update_stream2(
3925		    asd, stream_id,
3926		    asd->stream_env[stream_id].isys_info[1].input_format,
3927		    asd->stream_env[stream_id].isys_info[1].width,
3928		    asd->stream_env[stream_id].isys_info[1].height);
3929	}
3930
3931	/* Compatibility for sensors which provide no media bus code
3932	 * in s_mbus_framefmt() nor support pad formats. */
3933	if (mipi_info && mipi_info->input_format != -1) {
3934		bayer_order = mipi_info->raw_bayer_order;
3935
3936		/* Input stream config is still needs configured */
3937		/* TODO: Check if this is necessary */
3938		fc = atomisp_find_in_fmt_conv_by_atomisp_in_fmt(
3939			 mipi_info->input_format);
3940		if (!fc)
3941			return -EINVAL;
3942		input_format = fc->atomisp_in_fmt;
3943		metadata_format = mipi_info->metadata_format;
3944		metadata_width = mipi_info->metadata_width;
3945		metadata_height = mipi_info->metadata_height;
3946	} else {
3947		struct v4l2_mbus_framefmt *sink;
3948
3949		sink = atomisp_subdev_get_ffmt(&asd->subdev, NULL,
3950					       V4L2_SUBDEV_FORMAT_ACTIVE,
3951					       ATOMISP_SUBDEV_PAD_SINK);
3952		fc = atomisp_find_in_fmt_conv(sink->code);
3953		if (!fc)
3954			return -EINVAL;
3955		input_format = fc->atomisp_in_fmt;
3956		bayer_order = fc->bayer_order;
3957	}
3958
3959	atomisp_css_input_set_format(asd, stream_id, input_format);
3960	atomisp_css_input_set_bayer_order(asd, stream_id, bayer_order);
3961
3962	fc = atomisp_find_in_fmt_conv_by_atomisp_in_fmt(metadata_format);
3963	if (!fc)
3964		return -EINVAL;
3965
3966	input_format = fc->atomisp_in_fmt;
3967	mipi_port = atomisp_port_to_mipi_port(isp, input->port);
3968	atomisp_css_input_configure_port(asd, mipi_port,
3969					 isp->sensor_lanes[mipi_port],
3970					 0xffff4, mipi_freq,
3971					 input_format,
3972					 metadata_width, metadata_height);
3973	return 0;
3974}
3975
3976static int configure_pp_input_nop(struct atomisp_sub_device *asd,
3977				  unsigned int width, unsigned int height)
3978{
3979	return 0;
3980}
3981
3982static int configure_output_nop(struct atomisp_sub_device *asd,
3983				unsigned int width, unsigned int height,
3984				unsigned int min_width,
3985				enum ia_css_frame_format sh_fmt)
3986{
3987	return 0;
3988}
3989
3990static int get_frame_info_nop(struct atomisp_sub_device *asd,
3991			      struct ia_css_frame_info *finfo)
3992{
3993	return 0;
3994}
3995
3996/*
3997 * Resets CSS parameters that depend on input resolution.
3998 *
3999 * Update params like CSS RAW binning, 2ppc mode and pp_input
4000 * which depend on input size, but are not automatically
4001 * handled in CSS when the input resolution is changed.
4002 */
4003static int css_input_resolution_changed(struct atomisp_sub_device *asd,
4004					struct v4l2_mbus_framefmt *ffmt)
4005{
4006	struct atomisp_metadata_buf *md_buf = NULL, *_md_buf;
4007	unsigned int i;
4008
4009	dev_dbg(asd->isp->dev, "css_input_resolution_changed to %ux%u\n",
4010		ffmt->width, ffmt->height);
4011
4012	if (IS_ISP2401)
4013		atomisp_css_input_set_two_pixels_per_clock(asd, false);
4014	else
4015		atomisp_css_input_set_two_pixels_per_clock(asd, true);
4016
4017	/*
4018	 * If sensor input changed, which means metadata resolution changed
4019	 * together. Release all metadata buffers here to let it re-allocated
4020	 * next time in reqbufs.
4021	 */
4022	for (i = 0; i < ATOMISP_METADATA_TYPE_NUM; i++) {
4023		list_for_each_entry_safe(md_buf, _md_buf, &asd->metadata[i],
4024					 list) {
4025			atomisp_css_free_metadata_buffer(md_buf);
4026			list_del(&md_buf->list);
4027			kfree(md_buf);
4028		}
4029	}
4030	return 0;
4031
4032	/*
4033	 * TODO: atomisp_css_preview_configure_pp_input() not
4034	 *       reset due to CSS bug tracked as PSI BZ 115124
4035	 */
4036}
4037
4038static int atomisp_set_fmt_to_isp(struct video_device *vdev,
4039				  struct ia_css_frame_info *output_info,
4040				  const struct v4l2_pix_format *pix)
4041{
4042	struct camera_mipi_info *mipi_info;
4043	struct atomisp_device *isp = video_get_drvdata(vdev);
4044	struct atomisp_sub_device *asd = atomisp_to_video_pipe(vdev)->asd;
4045	struct atomisp_input_subdev *input = &isp->inputs[asd->input_curr];
4046	const struct atomisp_format_bridge *format;
4047	struct v4l2_rect *isp_sink_crop;
4048	enum ia_css_pipe_id pipe_id;
4049	int (*configure_output)(struct atomisp_sub_device *asd,
4050				unsigned int width, unsigned int height,
4051				unsigned int min_width,
4052				enum ia_css_frame_format sh_fmt) =
4053				    configure_output_nop;
4054	int (*get_frame_info)(struct atomisp_sub_device *asd,
4055			      struct ia_css_frame_info *finfo) =
4056				  get_frame_info_nop;
4057	int (*configure_pp_input)(struct atomisp_sub_device *asd,
4058				  unsigned int width, unsigned int height) =
4059				      configure_pp_input_nop;
4060	const struct atomisp_in_fmt_conv *fc = NULL;
4061	struct v4l2_mbus_framefmt *ffmt;
4062	int ret, i;
4063
4064	isp_sink_crop = atomisp_subdev_get_rect(
4065			    &asd->subdev, NULL, V4L2_SUBDEV_FORMAT_ACTIVE,
4066			    ATOMISP_SUBDEV_PAD_SINK, V4L2_SEL_TGT_CROP);
4067
4068	format = atomisp_get_format_bridge(pix->pixelformat);
4069	if (!format)
4070		return -EINVAL;
4071
4072	mipi_info = atomisp_to_sensor_mipi_info(input->sensor);
4073
4074	if (atomisp_set_sensor_mipi_to_isp(asd, ATOMISP_INPUT_STREAM_GENERAL,
4075					   mipi_info))
4076		return -EINVAL;
4077
4078	if (mipi_info)
4079		fc = atomisp_find_in_fmt_conv_by_atomisp_in_fmt(mipi_info->input_format);
4080	if (!fc) {
4081		ffmt = atomisp_subdev_get_ffmt(&asd->subdev, NULL,
4082					       V4L2_SUBDEV_FORMAT_ACTIVE,
4083					       ATOMISP_SUBDEV_PAD_SINK);
4084		fc = atomisp_find_in_fmt_conv(ffmt->code);
4085	}
4086	if (!fc)
4087		return -EINVAL;
4088
4089	if (format->sh_fmt == IA_CSS_FRAME_FORMAT_RAW &&
4090	    raw_output_format_match_input(fc->atomisp_in_fmt, pix->pixelformat))
4091		return -EINVAL;
4092
4093	/*
4094	 * Configure viewfinder also when vfpp is disabled: the
4095	 * CSS still requires viewfinder configuration.
4096	 */
4097	{
4098		u32 width, height;
4099
4100		if (pix->width < 640 || pix->height < 480) {
4101			width = pix->width;
4102			height = pix->height;
4103		} else {
4104			width = 640;
4105			height = 480;
4106		}
4107
4108		if (asd->run_mode->val == ATOMISP_RUN_MODE_VIDEO ||
4109		    asd->vfpp->val == ATOMISP_VFPP_DISABLE_SCALER) {
4110			atomisp_css_video_configure_viewfinder(asd, width, height, 0,
4111							       IA_CSS_FRAME_FORMAT_NV12);
4112		} else if (asd->run_mode->val == ATOMISP_RUN_MODE_STILL_CAPTURE ||
4113			   asd->vfpp->val == ATOMISP_VFPP_DISABLE_LOWLAT) {
4114			atomisp_css_capture_configure_viewfinder(asd, width, height, 0,
4115								 IA_CSS_FRAME_FORMAT_NV12);
4116		}
4117	}
4118
4119	atomisp_css_input_set_mode(asd, IA_CSS_INPUT_MODE_BUFFERED_SENSOR);
4120
4121	for (i = 0; i < IA_CSS_PIPE_ID_NUM; i++)
4122		asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].pipe_extra_configs[i].disable_vf_pp = asd->vfpp->val != ATOMISP_VFPP_ENABLE;
4123
4124	/* ISP2401 new input system need to use copy pipe */
4125	if (asd->copy_mode) {
4126		pipe_id = IA_CSS_PIPE_ID_COPY;
4127		atomisp_css_capture_enable_online(asd, ATOMISP_INPUT_STREAM_GENERAL, false);
4128	} else if (asd->vfpp->val == ATOMISP_VFPP_DISABLE_SCALER) {
4129		/* video same in continuouscapture and online modes */
4130		configure_output = atomisp_css_video_configure_output;
4131		get_frame_info = atomisp_css_video_get_output_frame_info;
4132		pipe_id = IA_CSS_PIPE_ID_VIDEO;
4133	} else if (asd->run_mode->val == ATOMISP_RUN_MODE_VIDEO) {
4134		configure_output = atomisp_css_video_configure_output;
4135		get_frame_info = atomisp_css_video_get_output_frame_info;
4136		pipe_id = IA_CSS_PIPE_ID_VIDEO;
4137	} else if (asd->run_mode->val == ATOMISP_RUN_MODE_PREVIEW) {
4138		configure_output = atomisp_css_preview_configure_output;
4139		get_frame_info = atomisp_css_preview_get_output_frame_info;
4140		configure_pp_input = atomisp_css_preview_configure_pp_input;
4141		pipe_id = IA_CSS_PIPE_ID_PREVIEW;
4142	} else {
4143		if (format->sh_fmt == IA_CSS_FRAME_FORMAT_RAW) {
4144			atomisp_css_capture_set_mode(asd, IA_CSS_CAPTURE_MODE_RAW);
4145			atomisp_css_enable_dz(asd, false);
4146		} else {
4147			atomisp_update_capture_mode(asd);
4148		}
4149
4150		/* in case of ANR, force capture pipe to offline mode */
4151		atomisp_css_capture_enable_online(asd, ATOMISP_INPUT_STREAM_GENERAL,
4152						  !asd->params.low_light);
4153
4154		configure_output = atomisp_css_capture_configure_output;
4155		get_frame_info = atomisp_css_capture_get_output_frame_info;
4156		configure_pp_input = atomisp_css_capture_configure_pp_input;
4157		pipe_id = IA_CSS_PIPE_ID_CAPTURE;
4158
4159		if (asd->run_mode->val != ATOMISP_RUN_MODE_STILL_CAPTURE) {
4160			dev_err(isp->dev,
4161				"Need to set the running mode first\n");
4162			asd->run_mode->val = ATOMISP_RUN_MODE_STILL_CAPTURE;
4163		}
4164	}
4165
4166	if (asd->copy_mode)
4167		ret = atomisp_css_copy_configure_output(asd, ATOMISP_INPUT_STREAM_GENERAL,
4168							pix->width, pix->height,
4169							format->planar ? pix->bytesperline :
4170							pix->bytesperline * 8 / format->depth,
4171							format->sh_fmt);
4172	else
4173		ret = configure_output(asd, pix->width, pix->height,
4174				       format->planar ? pix->bytesperline :
4175				       pix->bytesperline * 8 / format->depth,
4176				       format->sh_fmt);
4177	if (ret) {
4178		dev_err(isp->dev, "configure_output %ux%u, format %8.8x\n",
4179			pix->width, pix->height, format->sh_fmt);
4180		return -EINVAL;
4181	}
4182
4183	ret = configure_pp_input(asd, isp_sink_crop->width, isp_sink_crop->height);
4184	if (ret) {
4185		dev_err(isp->dev, "configure_pp_input %ux%u\n",
4186			isp_sink_crop->width,
4187			isp_sink_crop->height);
4188		return -EINVAL;
4189	}
4190	if (asd->copy_mode)
4191		ret = atomisp_css_copy_get_output_frame_info(asd,
4192							     ATOMISP_INPUT_STREAM_GENERAL,
4193							     output_info);
4194	else
4195		ret = get_frame_info(asd, output_info);
4196	if (ret) {
4197		dev_err(isp->dev, "__get_frame_info %ux%u (padded to %u) returned %d\n",
4198			pix->width, pix->height, pix->bytesperline, ret);
4199		return ret;
4200	}
4201
4202	atomisp_update_grid_info(asd, pipe_id);
4203	return 0;
4204}
4205
4206static void atomisp_get_dis_envelop(struct atomisp_sub_device *asd,
4207				    unsigned int width, unsigned int height,
4208				    unsigned int *dvs_env_w, unsigned int *dvs_env_h)
4209{
4210	if (asd->params.video_dis_en &&
4211	    asd->run_mode->val == ATOMISP_RUN_MODE_VIDEO) {
4212		/* envelope is 20% of the output resolution */
4213		/*
4214		 * dvs envelope cannot be round up.
4215		 * it would cause ISP timeout and color switch issue
4216		 */
4217		*dvs_env_w = rounddown(width / 5, ATOM_ISP_STEP_WIDTH);
4218		*dvs_env_h = rounddown(height / 5, ATOM_ISP_STEP_HEIGHT);
4219	}
4220
4221	asd->params.dis_proj_data_valid = false;
4222	asd->params.css_update_params_needed = true;
4223}
4224
4225static void atomisp_check_copy_mode(struct atomisp_sub_device *asd,
4226				    const struct v4l2_pix_format *f)
4227{
4228	struct v4l2_mbus_framefmt *sink, *src;
4229
4230	if (!IS_ISP2401) {
4231		/* Only used for the new input system */
4232		asd->copy_mode = false;
4233		return;
4234	}
4235
4236	sink = atomisp_subdev_get_ffmt(&asd->subdev, NULL,
4237				       V4L2_SUBDEV_FORMAT_ACTIVE, ATOMISP_SUBDEV_PAD_SINK);
4238	src = atomisp_subdev_get_ffmt(&asd->subdev, NULL,
4239				      V4L2_SUBDEV_FORMAT_ACTIVE, ATOMISP_SUBDEV_PAD_SOURCE);
4240
4241	if (sink->code == src->code && sink->width == f->width && sink->height == f->height)
4242		asd->copy_mode = true;
4243	else
4244		asd->copy_mode = false;
4245
4246	dev_dbg(asd->isp->dev, "copy_mode: %d\n", asd->copy_mode);
4247}
4248
4249static int atomisp_set_fmt_to_snr(struct video_device *vdev, const struct v4l2_pix_format *f,
4250				  unsigned int dvs_env_w, unsigned int dvs_env_h)
4251{
4252	struct atomisp_video_pipe *pipe = atomisp_to_video_pipe(vdev);
4253	struct atomisp_sub_device *asd = pipe->asd;
4254	struct atomisp_device *isp = asd->isp;
4255	const struct atomisp_format_bridge *format;
4256	struct v4l2_mbus_framefmt req_ffmt, ffmt = { };
4257	struct atomisp_input_stream_info *stream_info =
4258	    (struct atomisp_input_stream_info *)&ffmt.reserved;
4259	int ret;
4260
4261	format = atomisp_get_format_bridge(f->pixelformat);
4262	if (!format)
4263		return -EINVAL;
4264
4265	v4l2_fill_mbus_format(&ffmt, f, format->mbus_code);
4266	ffmt.height += asd->sink_pad_padding_h + dvs_env_h;
4267	ffmt.width += asd->sink_pad_padding_w + dvs_env_w;
4268
4269	dev_dbg(isp->dev, "s_mbus_fmt: ask %ux%u (padding %ux%u, dvs %ux%u)\n",
4270		ffmt.width, ffmt.height, asd->sink_pad_padding_w, asd->sink_pad_padding_h,
4271		dvs_env_w, dvs_env_h);
4272
4273	__atomisp_init_stream_info(ATOMISP_INPUT_STREAM_GENERAL, stream_info);
4274
4275	req_ffmt = ffmt;
4276
4277	/* Disable dvs if resolution can't be supported by sensor */
4278	if (asd->params.video_dis_en && asd->run_mode->val == ATOMISP_RUN_MODE_VIDEO) {
4279		ret = atomisp_set_sensor_crop_and_fmt(isp, &ffmt, V4L2_SUBDEV_FORMAT_TRY);
4280		if (ret)
4281			return ret;
4282
4283		dev_dbg(isp->dev, "video dis: sensor width: %d, height: %d\n",
4284			ffmt.width, ffmt.height);
4285
4286		if (ffmt.width < req_ffmt.width ||
4287		    ffmt.height < req_ffmt.height) {
4288			req_ffmt.height -= dvs_env_h;
4289			req_ffmt.width -= dvs_env_w;
4290			ffmt = req_ffmt;
4291			dev_warn(isp->dev,
4292				 "can not enable video dis due to sensor limitation.");
4293			asd->params.video_dis_en = false;
4294		}
4295	}
4296
4297	ret = atomisp_set_sensor_crop_and_fmt(isp, &ffmt, V4L2_SUBDEV_FORMAT_ACTIVE);
4298	if (ret)
4299		return ret;
4300
4301	__atomisp_update_stream_env(asd, ATOMISP_INPUT_STREAM_GENERAL, stream_info);
4302
4303	dev_dbg(isp->dev, "sensor width: %d, height: %d\n",
4304		ffmt.width, ffmt.height);
4305
4306	if (ffmt.width < ATOM_ISP_STEP_WIDTH ||
4307	    ffmt.height < ATOM_ISP_STEP_HEIGHT)
4308		return -EINVAL;
4309
4310	if (asd->params.video_dis_en && asd->run_mode->val == ATOMISP_RUN_MODE_VIDEO &&
4311	    (ffmt.width < req_ffmt.width || ffmt.height < req_ffmt.height)) {
4312		dev_warn(isp->dev,
4313			 "can not enable video dis due to sensor limitation.");
4314		asd->params.video_dis_en = false;
4315	}
4316
4317	atomisp_subdev_set_ffmt(&asd->subdev, NULL,
4318				V4L2_SUBDEV_FORMAT_ACTIVE,
4319				ATOMISP_SUBDEV_PAD_SINK, &ffmt);
4320
4321	return css_input_resolution_changed(asd, &ffmt);
4322}
4323
4324int atomisp_set_fmt(struct video_device *vdev, struct v4l2_format *f)
4325{
4326	struct atomisp_device *isp = video_get_drvdata(vdev);
4327	struct atomisp_video_pipe *pipe = atomisp_to_video_pipe(vdev);
4328	struct atomisp_sub_device *asd = pipe->asd;
4329	const struct atomisp_format_bridge *format_bridge;
4330	const struct atomisp_format_bridge *snr_format_bridge;
4331	struct ia_css_frame_info output_info;
4332	unsigned int dvs_env_w = 0, dvs_env_h = 0;
4333	struct v4l2_mbus_framefmt isp_source_fmt = {0};
4334	struct v4l2_rect isp_sink_crop;
4335	int ret;
4336
4337	ret = atomisp_pipe_check(pipe, true);
4338	if (ret)
4339		return ret;
4340
4341	dev_dbg(isp->dev,
4342		"setting resolution %ux%u bytesperline %u\n",
4343		f->fmt.pix.width, f->fmt.pix.height, f->fmt.pix.bytesperline);
4344
4345	/* Ensure that the resolution is equal or below the maximum supported */
4346	ret = atomisp_try_fmt(isp, &f->fmt.pix, &format_bridge, &snr_format_bridge);
4347	if (ret)
4348		return ret;
4349
4350	pipe->sh_fmt = format_bridge->sh_fmt;
4351	pipe->pix.pixelformat = format_bridge->pixelformat;
4352
4353	atomisp_subdev_get_ffmt(&asd->subdev, NULL,
4354				V4L2_SUBDEV_FORMAT_ACTIVE,
4355				ATOMISP_SUBDEV_PAD_SINK)->code =
4356				    snr_format_bridge->mbus_code;
4357
4358	isp_source_fmt.code = format_bridge->mbus_code;
4359	atomisp_subdev_set_ffmt(&asd->subdev, NULL,
4360				V4L2_SUBDEV_FORMAT_ACTIVE,
4361				ATOMISP_SUBDEV_PAD_SOURCE, &isp_source_fmt);
4362
4363	if (atomisp_subdev_format_conversion(asd)) {
4364		atomisp_get_padding(isp, f->fmt.pix.width, f->fmt.pix.height,
4365				    &asd->sink_pad_padding_w, &asd->sink_pad_padding_h);
4366	} else {
4367		asd->sink_pad_padding_w = 0;
4368		asd->sink_pad_padding_h = 0;
4369	}
4370
4371	atomisp_get_dis_envelop(asd, f->fmt.pix.width, f->fmt.pix.height,
4372				&dvs_env_w, &dvs_env_h);
4373
4374	ret = atomisp_set_fmt_to_snr(vdev, &f->fmt.pix, dvs_env_w, dvs_env_h);
4375	if (ret) {
4376		dev_warn(isp->dev,
4377			 "Set format to sensor failed with %d\n", ret);
4378		return -EINVAL;
4379	}
4380
4381	atomisp_csi_lane_config(isp);
4382
4383	atomisp_check_copy_mode(asd, &f->fmt.pix);
4384
4385	isp_sink_crop = *atomisp_subdev_get_rect(&asd->subdev, NULL,
4386			V4L2_SUBDEV_FORMAT_ACTIVE,
4387			ATOMISP_SUBDEV_PAD_SINK,
4388			V4L2_SEL_TGT_CROP);
4389
4390	/* Try to enable YUV downscaling if ISP input is 10 % (either
4391	 * width or height) bigger than the desired result. */
4392	if (!IS_MOFD ||
4393	    isp_sink_crop.width * 9 / 10 < f->fmt.pix.width ||
4394	    isp_sink_crop.height * 9 / 10 < f->fmt.pix.height ||
4395	    (atomisp_subdev_format_conversion(asd) &&
4396	     (asd->run_mode->val == ATOMISP_RUN_MODE_VIDEO ||
4397	      asd->vfpp->val == ATOMISP_VFPP_DISABLE_SCALER))) {
4398		isp_sink_crop.width = f->fmt.pix.width;
4399		isp_sink_crop.height = f->fmt.pix.height;
4400
4401		atomisp_subdev_set_selection(&asd->subdev, NULL,
4402					     V4L2_SUBDEV_FORMAT_ACTIVE,
4403					     ATOMISP_SUBDEV_PAD_SOURCE, V4L2_SEL_TGT_COMPOSE,
4404					     0, &isp_sink_crop);
4405	} else {
4406		struct v4l2_rect main_compose = {0};
4407
4408		main_compose.width = isp_sink_crop.width;
4409		main_compose.height =
4410		    DIV_ROUND_UP(main_compose.width * f->fmt.pix.height,
4411				 f->fmt.pix.width);
4412		if (main_compose.height > isp_sink_crop.height) {
4413			main_compose.height = isp_sink_crop.height;
4414			main_compose.width =
4415			    DIV_ROUND_UP(main_compose.height *
4416					 f->fmt.pix.width,
4417					 f->fmt.pix.height);
4418		}
4419
4420		atomisp_subdev_set_selection(&asd->subdev, NULL,
4421					     V4L2_SUBDEV_FORMAT_ACTIVE,
4422					     ATOMISP_SUBDEV_PAD_SOURCE,
4423					     V4L2_SEL_TGT_COMPOSE, 0,
4424					     &main_compose);
4425	}
4426
4427	ret = atomisp_set_fmt_to_isp(vdev, &output_info, &f->fmt.pix);
4428	if (ret) {
4429		dev_warn(isp->dev, "Can't set format on ISP. Error %d\n", ret);
4430		return -EINVAL;
4431	}
4432
4433	atomisp_fill_pix_format(&pipe->pix, f->fmt.pix.width, f->fmt.pix.height, format_bridge);
4434
4435	f->fmt.pix = pipe->pix;
4436
4437	dev_dbg(isp->dev, "%s: %dx%d, image size: %d, %d bytes per line\n",
4438		__func__,
4439		f->fmt.pix.width, f->fmt.pix.height,
4440		f->fmt.pix.sizeimage, f->fmt.pix.bytesperline);
4441
4442	return 0;
4443}
4444
4445int atomisp_set_shading_table(struct atomisp_sub_device *asd,
4446			      struct atomisp_shading_table *user_shading_table)
4447{
4448	struct ia_css_shading_table *shading_table;
4449	struct ia_css_shading_table *free_table;
4450	unsigned int len_table;
4451	int i;
4452	int ret = 0;
4453
4454	if (!user_shading_table)
4455		return -EINVAL;
4456
4457	if (!user_shading_table->enable) {
4458		asd->params.config.shading_table = NULL;
4459		asd->params.sc_en = false;
4460		return 0;
4461	}
4462
4463	/* If enabling, all tables must be set */
4464	for (i = 0; i < ATOMISP_NUM_SC_COLORS; i++) {
4465		if (!user_shading_table->data[i])
4466			return -EINVAL;
4467	}
4468
4469	/* Shading table size per color */
4470	if (user_shading_table->width > SH_CSS_MAX_SCTBL_WIDTH_PER_COLOR ||
4471	    user_shading_table->height > SH_CSS_MAX_SCTBL_HEIGHT_PER_COLOR)
4472		return -EINVAL;
4473
4474	shading_table = atomisp_css_shading_table_alloc(
4475			    user_shading_table->width, user_shading_table->height);
4476	if (!shading_table)
4477		return -ENOMEM;
4478
4479	len_table = user_shading_table->width * user_shading_table->height *
4480		    ATOMISP_SC_TYPE_SIZE;
4481	for (i = 0; i < ATOMISP_NUM_SC_COLORS; i++) {
4482		ret = copy_from_user(shading_table->data[i],
4483				     (void __user *)user_shading_table->data[i],
4484				     len_table);
4485		if (ret) {
4486			free_table = shading_table;
4487			ret = -EFAULT;
4488			goto out;
4489		}
4490	}
4491	shading_table->sensor_width = user_shading_table->sensor_width;
4492	shading_table->sensor_height = user_shading_table->sensor_height;
4493	shading_table->fraction_bits = user_shading_table->fraction_bits;
4494
4495	free_table = asd->params.css_param.shading_table;
4496	asd->params.css_param.shading_table = shading_table;
4497	asd->params.config.shading_table = shading_table;
4498	asd->params.sc_en = true;
4499
4500out:
4501	if (free_table)
4502		atomisp_css_shading_table_free(free_table);
4503
4504	return ret;
4505}
4506
4507static int __checking_exp_id(struct atomisp_sub_device *asd, int exp_id)
4508{
4509	struct atomisp_device *isp = asd->isp;
4510
4511	if (!asd->enable_raw_buffer_lock->val) {
4512		dev_warn(isp->dev, "%s Raw Buffer Lock is disable.\n", __func__);
4513		return -EINVAL;
4514	}
4515	if (!asd->streaming) {
4516		dev_err(isp->dev, "%s streaming %d invalid exp_id %d.\n",
4517			__func__, exp_id, asd->streaming);
4518		return -EINVAL;
4519	}
4520	if ((exp_id > ATOMISP_MAX_EXP_ID) || (exp_id <= 0)) {
4521		dev_err(isp->dev, "%s exp_id %d invalid.\n", __func__, exp_id);
4522		return -EINVAL;
4523	}
4524	return 0;
4525}
4526
4527void atomisp_init_raw_buffer_bitmap(struct atomisp_sub_device *asd)
4528{
4529	unsigned long flags;
4530
4531	spin_lock_irqsave(&asd->raw_buffer_bitmap_lock, flags);
4532	memset(asd->raw_buffer_bitmap, 0, sizeof(asd->raw_buffer_bitmap));
4533	asd->raw_buffer_locked_count = 0;
4534	spin_unlock_irqrestore(&asd->raw_buffer_bitmap_lock, flags);
4535}
4536
4537static int __is_raw_buffer_locked(struct atomisp_sub_device *asd, int exp_id)
4538{
4539	int *bitmap, bit;
4540	unsigned long flags;
4541	int ret;
4542
4543	if (__checking_exp_id(asd, exp_id))
4544		return -EINVAL;
4545
4546	bitmap = asd->raw_buffer_bitmap + exp_id / 32;
4547	bit = exp_id % 32;
4548	spin_lock_irqsave(&asd->raw_buffer_bitmap_lock, flags);
4549	ret = ((*bitmap) & (1 << bit));
4550	spin_unlock_irqrestore(&asd->raw_buffer_bitmap_lock, flags);
4551	return !ret;
4552}
4553
4554static int __clear_raw_buffer_bitmap(struct atomisp_sub_device *asd, int exp_id)
4555{
4556	int *bitmap, bit;
4557	unsigned long flags;
4558
4559	if (__is_raw_buffer_locked(asd, exp_id))
4560		return -EINVAL;
4561
4562	bitmap = asd->raw_buffer_bitmap + exp_id / 32;
4563	bit = exp_id % 32;
4564	spin_lock_irqsave(&asd->raw_buffer_bitmap_lock, flags);
4565	(*bitmap) &= ~(1 << bit);
4566	asd->raw_buffer_locked_count--;
4567	spin_unlock_irqrestore(&asd->raw_buffer_bitmap_lock, flags);
4568
4569	dev_dbg(asd->isp->dev, "%s: exp_id %d,  raw_buffer_locked_count %d\n",
4570		__func__, exp_id, asd->raw_buffer_locked_count);
4571	return 0;
4572}
4573
4574int atomisp_exp_id_capture(struct atomisp_sub_device *asd, int *exp_id)
4575{
4576	struct atomisp_device *isp = asd->isp;
4577	int value = *exp_id;
4578	int ret;
4579
4580	lockdep_assert_held(&isp->mutex);
4581
4582	ret = __is_raw_buffer_locked(asd, value);
4583	if (ret) {
4584		dev_err(isp->dev, "%s exp_id %d invalid %d.\n", __func__, value, ret);
4585		return -EINVAL;
4586	}
4587
4588	dev_dbg(isp->dev, "%s exp_id %d\n", __func__, value);
4589	ret = atomisp_css_exp_id_capture(asd, value);
4590	if (ret) {
4591		dev_err(isp->dev, "%s exp_id %d failed.\n", __func__, value);
4592		return -EIO;
4593	}
4594	return 0;
4595}
4596
4597int atomisp_exp_id_unlock(struct atomisp_sub_device *asd, int *exp_id)
4598{
4599	struct atomisp_device *isp = asd->isp;
4600	int value = *exp_id;
4601	int ret;
4602
4603	lockdep_assert_held(&isp->mutex);
4604
4605	ret = __clear_raw_buffer_bitmap(asd, value);
4606	if (ret) {
4607		dev_err(isp->dev, "%s exp_id %d invalid %d.\n", __func__, value, ret);
4608		return -EINVAL;
4609	}
4610
4611	dev_dbg(isp->dev, "%s exp_id %d\n", __func__, value);
4612	ret = atomisp_css_exp_id_unlock(asd, value);
4613	if (ret)
4614		dev_err(isp->dev, "%s exp_id %d failed, err %d.\n",
4615			__func__, value, ret);
4616
4617	return ret;
4618}
4619
4620int atomisp_enable_dz_capt_pipe(struct atomisp_sub_device *asd,
4621				unsigned int *enable)
4622{
4623	bool value;
4624
4625	if (!enable)
4626		return -EINVAL;
4627
4628	value = *enable > 0;
4629
4630	atomisp_en_dz_capt_pipe(asd, value);
4631
4632	return 0;
4633}
4634
4635int atomisp_inject_a_fake_event(struct atomisp_sub_device *asd, int *event)
4636{
4637	if (!event || !asd->streaming)
4638		return -EINVAL;
4639
4640	lockdep_assert_held(&asd->isp->mutex);
4641
4642	dev_dbg(asd->isp->dev, "%s: trying to inject a fake event 0x%x\n",
4643		__func__, *event);
4644
4645	switch (*event) {
4646	case V4L2_EVENT_FRAME_SYNC:
4647		atomisp_sof_event(asd);
4648		break;
4649	case V4L2_EVENT_FRAME_END:
4650		atomisp_eof_event(asd, 0);
4651		break;
4652	case V4L2_EVENT_ATOMISP_3A_STATS_READY:
4653		atomisp_3a_stats_ready_event(asd, 0);
4654		break;
4655	case V4L2_EVENT_ATOMISP_METADATA_READY:
4656		atomisp_metadata_ready_event(asd, 0);
4657		break;
4658	default:
4659		return -EINVAL;
4660	}
4661
4662	return 0;
4663}