drivers/staging/media/atomisp/pci/atomisp2/atomisp_cmd.c

   1 /*
   2  * Support for Medifield PNW Camera Imaging ISP subsystem.
   3  *
   4  * Copyright (c) 2010 Intel Corporation. All Rights Reserved.
   5  *
   6  * Copyright (c) 2010 Silicon Hive www.siliconhive.com.
   7  *
   8  * This program is free software; you can redistribute it and/or
   9  * modify it under the terms of the GNU General Public License version
  10  * 2 as published by the Free Software Foundation.
  11  *
  12  * This program is distributed in the hope that it will be useful,
  13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  15  * GNU General Public License for more details.
  16  *
  17  * You should have received a copy of the GNU General Public License
  18  * along with this program; if not, write to the Free Software
  19  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
  20  * 02110-1301, USA.
  21  *
  22  */
  23 #include <linux/firmware.h>
  24 #include <linux/pci.h>
  25 #include <linux/interrupt.h>
  26 #include <linux/kernel.h>
  27 #include <linux/kfifo.h>
  28 #include <linux/pm_runtime.h>
  29 #include <linux/timer.h>
  30 #include <asm/intel-mid.h>
  31
  32 #include <media/v4l2-event.h>
  33 #include <media/videobuf-vmalloc.h>
  34
  35 #define CREATE_TRACE_POINTS
  36 #include "atomisp_trace_event.h"
  37
  38 #include "atomisp_cmd.h"
  39 #include "atomisp_common.h"
  40 #include "atomisp_fops.h"
  41 #include "atomisp_internal.h"
  42 #include "atomisp_ioctl.h"
  43 #include "atomisp-regs.h"
  44 #include "atomisp_tables.h"
  45 #include "atomisp_acc.h"
  46 #include "atomisp_compat.h"
  47 #include "atomisp_subdev.h"
  48 #include "atomisp_dfs_tables.h"
  49
  50 #include "hrt/hive_isp_css_mm_hrt.h"
  51
  52 #include "sh_css_hrt.h"
  53 #include "sh_css_defs.h"
  54 #include "system_global.h"
  55 #include "sh_css_internal.h"
  56 #include "sh_css_sp.h"
  57 #include "gp_device.h"
  58 #include "device_access.h"
  59 #include "irq.h"
  60
  61 #include "ia_css_types.h"
  62 #include "ia_css_stream.h"
  63 #include "error_support.h"
  64 #include "hrt/bits.h"
  65
  66
  67 /* We should never need to run the flash for more than 2 frames.
  68  * At 15fps this means 133ms. We set the timeout a bit longer.
  69  * Each flash driver is supposed to set its own timeout, but
  70  * just in case someone else changed the timeout, we set it
  71  * here to make sure we don't damage the flash hardware. */
  72 #define FLASH_TIMEOUT 800 /* ms */
  73
  74 union host {
  75         struct {
  76                 void *kernel_ptr;
  77                 void __user *user_ptr;
  78                 int size;
  79         } scalar;
  80         struct {
  81                 void *hmm_ptr;
  82         } ptr;
  83 };
  84
  85 /*
  86  * get sensor:dis71430/ov2720 related info from v4l2_subdev->priv data field.
  87  * subdev->priv is set in mrst.c
  88  */
  89 struct camera_mipi_info *atomisp_to_sensor_mipi_info(struct v4l2_subdev *sd)
  90 {
  91         return (struct camera_mipi_info *)v4l2_get_subdev_hostdata(sd);
  92 }
  93
  94 /*
  95  * get struct atomisp_video_pipe from v4l2 video_device
  96  */
  97 struct atomisp_video_pipe *atomisp_to_video_pipe(struct video_device *dev)
  98 {
  99         return (struct atomisp_video_pipe *)
 100             container_of(dev, struct atomisp_video_pipe, vdev);
 101 }
 102
 103 /*
 104  * get struct atomisp_acc_pipe from v4l2 video_device
 105  */
 106 struct atomisp_acc_pipe *atomisp_to_acc_pipe(struct video_device *dev)
 107 {
 108         return (struct atomisp_acc_pipe *)
 109             container_of(dev, struct atomisp_acc_pipe, vdev);
 110 }
 111
 112 static unsigned short atomisp_get_sensor_fps(struct atomisp_sub_device *asd)
 113 {
 114         struct v4l2_subdev_frame_interval fi;
 115         struct atomisp_device *isp = asd->isp;
 116
 117         unsigned short fps = 0;
 118         int ret;
 119
 120         ret = v4l2_subdev_call(isp->inputs[asd->input_curr].camera,
 121                                video, g_frame_interval, &fi);
 122
 123         if (!ret && fi.interval.numerator)
 124                 fps = fi.interval.denominator / fi.interval.numerator;
 125
 126         return fps;
 127 }
 128
 129 /*
 130  * DFS progress is shown as follows:
 131  * 1. Target frequency is calculated according to FPS/Resolution/ISP running
 132  *    mode.
 133  * 2. Ratio is calculated using formula: 2 * HPLL / target frequency - 1
 134  *    with proper rounding.
 135  * 3. Set ratio to ISPFREQ40, 1 to FREQVALID and ISPFREQGUAR40
 136  *    to 200MHz in ISPSSPM1.
 137  * 4. Wait for FREQVALID to be cleared by P-Unit.
 138  * 5. Wait for field ISPFREQSTAT40 in ISPSSPM1 turn to ratio set in 3.
 139  */
 140 static int write_target_freq_to_hw(struct atomisp_device *isp,
 141                                    unsigned int new_freq)
 142 {
 143         unsigned int ratio, timeout, guar_ratio;
 144         u32 isp_sspm1 = 0;
 145         int i;
 146         if (!isp->hpll_freq) {
 147                 dev_err(isp->dev, "failed to get hpll_freq. no change to freq\n");
 148                 return -EINVAL;
 149         }
 150
 151         isp_sspm1 = intel_mid_msgbus_read32(PUNIT_PORT, ISPSSPM1);
 152         if (isp_sspm1 & ISP_FREQ_VALID_MASK) {
 153                 dev_dbg(isp->dev, "clearing ISPSSPM1 valid bit.\n");
 154                 intel_mid_msgbus_write32(PUNIT_PORT, ISPSSPM1,
 155                                     isp_sspm1 & ~(1 << ISP_FREQ_VALID_OFFSET));
 156         }
 157
 158         ratio = (2 * isp->hpll_freq + new_freq / 2) / new_freq - 1;
 159         guar_ratio = (2 * isp->hpll_freq + 200 / 2) / 200 - 1;
 160
 161         isp_sspm1 = intel_mid_msgbus_read32(PUNIT_PORT, ISPSSPM1);
 162         isp_sspm1 &= ~(0x1F << ISP_REQ_FREQ_OFFSET);
 163
 164         for (i = 0; i < ISP_DFS_TRY_TIMES; i++) {
 165                 intel_mid_msgbus_write32(PUNIT_PORT, ISPSSPM1,
 166                                    isp_sspm1
 167                                    | ratio << ISP_REQ_FREQ_OFFSET
 168                                    | 1 << ISP_FREQ_VALID_OFFSET
 169                                    | guar_ratio << ISP_REQ_GUAR_FREQ_OFFSET);
 170
 171                 isp_sspm1 = intel_mid_msgbus_read32(PUNIT_PORT, ISPSSPM1);
 172
 173                 timeout = 20;
 174                 while ((isp_sspm1 & ISP_FREQ_VALID_MASK) && timeout) {
 175                         isp_sspm1 = intel_mid_msgbus_read32(PUNIT_PORT, ISPSSPM1);
 176                         dev_dbg(isp->dev, "waiting for ISPSSPM1 valid bit to be 0.\n");
 177                         udelay(100);
 178                         timeout--;
 179                 }
 180
 181                 if (timeout != 0)
 182                         break;
 183         }
 184
 185         if (timeout == 0) {
 186                 dev_err(isp->dev, "DFS failed due to HW error.\n");
 187                 return -EINVAL;
 188         }
 189
 190         isp_sspm1 = intel_mid_msgbus_read32(PUNIT_PORT, ISPSSPM1);
 191         timeout = 10;
 192         while (((isp_sspm1 >> ISP_FREQ_STAT_OFFSET) != ratio) && timeout) {
 193                 isp_sspm1 = intel_mid_msgbus_read32(PUNIT_PORT, ISPSSPM1);
 194                 dev_dbg(isp->dev, "waiting for ISPSSPM1 status bit to be 0x%x.\n",
 195                         new_freq);
 196                 udelay(100);
 197                 timeout--;
 198         }
 199         if (timeout == 0) {
 200                 dev_err(isp->dev, "DFS target freq is rejected by HW.\n");
 201                 return -EINVAL;
 202         }
 203
 204         return 0;
 205 }
 206 int atomisp_freq_scaling(struct atomisp_device *isp,
 207                          enum atomisp_dfs_mode mode,
 208                          bool force)
 209 {
 210         /* FIXME! Only use subdev[0] status yet */
 211         struct atomisp_sub_device *asd = &isp->asd[0];
 212         const struct atomisp_dfs_config *dfs;
 213         unsigned int new_freq;
 214         struct atomisp_freq_scaling_rule curr_rules;
 215         int i, ret;
 216         unsigned short fps = 0;
 217
 218         if (isp->sw_contex.power_state != ATOM_ISP_POWER_UP) {
 219                 dev_err(isp->dev, "DFS cannot proceed due to no power.\n");
 220                 return -EINVAL;
 221         }
 222
 223         if ((isp->pdev->device & ATOMISP_PCI_DEVICE_SOC_MASK) ==
 224                 ATOMISP_PCI_DEVICE_SOC_CHT && ATOMISP_USE_YUVPP(asd))
 225                 isp->dfs = &dfs_config_cht_soc;
 226
 227         dfs = isp->dfs;
 228
 229         if (dfs->lowest_freq == 0 || dfs->max_freq_at_vmin == 0 ||
 230             dfs->highest_freq == 0 || dfs->dfs_table_size == 0 ||
 231             !dfs->dfs_table) {
 232                 dev_err(isp->dev, "DFS configuration is invalid.\n");
 233                 return -EINVAL;
 234         }
 235
 236         if (mode == ATOMISP_DFS_MODE_LOW) {
 237                 new_freq = dfs->lowest_freq;
 238                 goto done;
 239         }
 240
 241         if (mode == ATOMISP_DFS_MODE_MAX) {
 242                 new_freq = dfs->highest_freq;
 243                 goto done;
 244         }
 245
 246         fps = atomisp_get_sensor_fps(asd);
 247         if (fps == 0)
 248                 return -EINVAL;
 249
 250         curr_rules.width = asd->fmt[asd->capture_pad].fmt.width;
 251         curr_rules.height = asd->fmt[asd->capture_pad].fmt.height;
 252         curr_rules.fps = fps;
 253         curr_rules.run_mode = asd->run_mode->val;
 254         /*
 255          * For continuous mode, we need to make the capture setting applied
 256          * since preview mode, because there is no chance to do this when
 257          * starting image capture.
 258          */
 259         if (asd->continuous_mode->val) {
 260                 if (asd->run_mode->val == ATOMISP_RUN_MODE_VIDEO)
 261                         curr_rules.run_mode = ATOMISP_RUN_MODE_SDV;
 262                 else
 263                         curr_rules.run_mode =
 264                                 ATOMISP_RUN_MODE_CONTINUOUS_CAPTURE;
 265         }
 266
 267         /* search for the target frequency by looping freq rules*/
 268         for (i = 0; i < dfs->dfs_table_size; i++) {
 269                 if (curr_rules.width != dfs->dfs_table[i].width &&
 270                     dfs->dfs_table[i].width != ISP_FREQ_RULE_ANY)
 271                         continue;
 272                 if (curr_rules.height != dfs->dfs_table[i].height &&
 273                     dfs->dfs_table[i].height != ISP_FREQ_RULE_ANY)
 274                         continue;
 275                 if (curr_rules.fps != dfs->dfs_table[i].fps &&
 276                     dfs->dfs_table[i].fps != ISP_FREQ_RULE_ANY)
 277                         continue;
 278                 if (curr_rules.run_mode != dfs->dfs_table[i].run_mode &&
 279                     dfs->dfs_table[i].run_mode != ISP_FREQ_RULE_ANY)
 280                         continue;
 281                 break;
 282         }
 283
 284         if (i == dfs->dfs_table_size)
 285                 new_freq = dfs->max_freq_at_vmin;
 286         else
 287                 new_freq = dfs->dfs_table[i].isp_freq;
 288
 289 done:
 290         dev_dbg(isp->dev, "DFS target frequency=%d.\n", new_freq);
 291
 292         if ((new_freq == isp->sw_contex.running_freq) && !force)
 293                 return 0;
 294
 295         dev_dbg(isp->dev, "Programming DFS frequency to %d\n", new_freq);
 296
 297         ret = write_target_freq_to_hw(isp, new_freq);
 298         if (!ret) {
 299                 isp->sw_contex.running_freq = new_freq;
 300                 trace_ipu_pstate(new_freq, -1);
 301         }
 302         return ret;
 303 }
 304
 305 /*
 306  * reset and restore ISP
 307  */
 308 int atomisp_reset(struct atomisp_device *isp)
 309 {
 310         /* Reset ISP by power-cycling it */
 311         int ret = 0;
 312
 313         dev_dbg(isp->dev, "%s\n", __func__);
 314         atomisp_css_suspend(isp);
 315         ret = atomisp_runtime_suspend(isp->dev);
 316         if (ret < 0)
 317                 dev_err(isp->dev, "atomisp_runtime_suspend failed, %d\n", ret);
 318         ret = atomisp_mrfld_power_down(isp);
 319         if (ret < 0) {
 320                 dev_err(isp->dev, "can not disable ISP power\n");
 321         } else {
 322                 ret = atomisp_mrfld_power_up(isp);
 323                 if (ret < 0)
 324                         dev_err(isp->dev, "can not enable ISP power\n");
 325                 ret = atomisp_runtime_resume(isp->dev);
 326                 if (ret < 0)
 327                         dev_err(isp->dev, "atomisp_runtime_resume failed, %d\n", ret);
 328         }
 329         ret = atomisp_css_resume(isp);
 330         if (ret)
 331                 isp->isp_fatal_error = true;
 332
 333         return ret;
 334 }
 335
 336 /*
 337  * interrupt disable functions
 338  */
 339 static void disable_isp_irq(enum hrt_isp_css_irq irq)
 340 {
 341         irq_disable_channel(IRQ0_ID, irq);
 342
 343         if (irq != hrt_isp_css_irq_sp)
 344                 return;
 345
 346         cnd_sp_irq_enable(SP0_ID, false);
 347 }
 348
 349 /*
 350  * interrupt clean function
 351  */
 352 static void clear_isp_irq(enum hrt_isp_css_irq irq)
 353 {
 354         irq_clear_all(IRQ0_ID);
 355 }
 356
 357 void atomisp_msi_irq_init(struct atomisp_device *isp, struct pci_dev *dev)
 358 {
 359         u32 msg32;
 360         u16 msg16;
 361
 362         pci_read_config_dword(dev, PCI_MSI_CAPID, &msg32);
 363         msg32 |= 1 << MSI_ENABLE_BIT;
 364         pci_write_config_dword(dev, PCI_MSI_CAPID, msg32);
 365
 366         msg32 = (1 << INTR_IER) | (1 << INTR_IIR);
 367         pci_write_config_dword(dev, PCI_INTERRUPT_CTRL, msg32);
 368
 369         pci_read_config_word(dev, PCI_COMMAND, &msg16);
 370         msg16 |= (PCI_COMMAND_MEMORY | PCI_COMMAND_MASTER |
 371                   PCI_COMMAND_INTX_DISABLE);
 372         pci_write_config_word(dev, PCI_COMMAND, msg16);
 373 }
 374
 375 void atomisp_msi_irq_uninit(struct atomisp_device *isp, struct pci_dev *dev)
 376 {
 377         u32 msg32;
 378         u16 msg16;
 379
 380         pci_read_config_dword(dev, PCI_MSI_CAPID, &msg32);
 381         msg32 &=  ~(1 << MSI_ENABLE_BIT);
 382         pci_write_config_dword(dev, PCI_MSI_CAPID, msg32);
 383
 384         msg32 = 0x0;
 385         pci_write_config_dword(dev, PCI_INTERRUPT_CTRL, msg32);
 386
 387         pci_read_config_word(dev, PCI_COMMAND, &msg16);
 388         msg16 &= ~(PCI_COMMAND_MASTER);
 389         pci_write_config_word(dev, PCI_COMMAND, msg16);
 390 }
 391
 392 static void atomisp_sof_event(struct atomisp_sub_device *asd)
 393 {
 394         struct v4l2_event event = {0};
 395
 396         event.type = V4L2_EVENT_FRAME_SYNC;
 397         event.u.frame_sync.frame_sequence = atomic_read(&asd->sof_count);
 398
 399         v4l2_event_queue(asd->subdev.devnode, &event);
 400 }
 401
 402 void atomisp_eof_event(struct atomisp_sub_device *asd, uint8_t exp_id)
 403 {
 404         struct v4l2_event event = {0};
 405
 406         event.type = V4L2_EVENT_FRAME_END;
 407         event.u.frame_sync.frame_sequence = exp_id;
 408
 409         v4l2_event_queue(asd->subdev.devnode, &event);
 410 }
 411
 412 static void atomisp_3a_stats_ready_event(struct atomisp_sub_device *asd, uint8_t exp_id)
 413 {
 414         struct v4l2_event event = {0};
 415
 416         event.type = V4L2_EVENT_ATOMISP_3A_STATS_READY;
 417         event.u.frame_sync.frame_sequence = exp_id;
 418
 419         v4l2_event_queue(asd->subdev.devnode, &event);
 420 }
 421
 422 static void atomisp_metadata_ready_event(struct atomisp_sub_device *asd,
 423                                          enum atomisp_metadata_type md_type)
 424 {
 425         struct v4l2_event event = {0};
 426
 427         event.type = V4L2_EVENT_ATOMISP_METADATA_READY;
 428         event.u.data[0] = md_type;
 429
 430         v4l2_event_queue(asd->subdev.devnode, &event);
 431 }
 432
 433 static void atomisp_reset_event(struct atomisp_sub_device *asd)
 434 {
 435         struct v4l2_event event = {0};
 436
 437         event.type = V4L2_EVENT_ATOMISP_CSS_RESET;
 438
 439         v4l2_event_queue(asd->subdev.devnode, &event);
 440 }
 441
 442
 443 static void print_csi_rx_errors(enum ia_css_csi2_port port,
 444                                 struct atomisp_device *isp)
 445 {
 446         u32 infos = 0;
 447
 448         atomisp_css_rx_get_irq_info(port, &infos);
 449
 450         dev_err(isp->dev, "CSI Receiver port %d errors:\n", port);
 451         if (infos & CSS_RX_IRQ_INFO_BUFFER_OVERRUN)
 452                 dev_err(isp->dev, "  buffer overrun");
 453         if (infos & CSS_RX_IRQ_INFO_ERR_SOT)
 454                 dev_err(isp->dev, "  start-of-transmission error");
 455         if (infos & CSS_RX_IRQ_INFO_ERR_SOT_SYNC)
 456                 dev_err(isp->dev, "  start-of-transmission sync error");
 457         if (infos & CSS_RX_IRQ_INFO_ERR_CONTROL)
 458                 dev_err(isp->dev, "  control error");
 459         if (infos & CSS_RX_IRQ_INFO_ERR_ECC_DOUBLE)
 460                 dev_err(isp->dev, "  2 or more ECC errors");
 461         if (infos & CSS_RX_IRQ_INFO_ERR_CRC)
 462                 dev_err(isp->dev, "  CRC mismatch");
 463         if (infos & CSS_RX_IRQ_INFO_ERR_UNKNOWN_ID)
 464                 dev_err(isp->dev, "  unknown error");
 465         if (infos & CSS_RX_IRQ_INFO_ERR_FRAME_SYNC)
 466                 dev_err(isp->dev, "  frame sync error");
 467         if (infos & CSS_RX_IRQ_INFO_ERR_FRAME_DATA)
 468                 dev_err(isp->dev, "  frame data error");
 469         if (infos & CSS_RX_IRQ_INFO_ERR_DATA_TIMEOUT)
 470                 dev_err(isp->dev, "  data timeout");
 471         if (infos & CSS_RX_IRQ_INFO_ERR_UNKNOWN_ESC)
 472                 dev_err(isp->dev, "  unknown escape command entry");
 473         if (infos & CSS_RX_IRQ_INFO_ERR_LINE_SYNC)
 474                 dev_err(isp->dev, "  line sync error");
 475 }
 476
 477 /* Clear irq reg */
 478 static void clear_irq_reg(struct atomisp_device *isp)
 479 {
 480         u32 msg_ret;
 481         pci_read_config_dword(isp->pdev, PCI_INTERRUPT_CTRL, &msg_ret);
 482         msg_ret |= 1 << INTR_IIR;
 483         pci_write_config_dword(isp->pdev, PCI_INTERRUPT_CTRL, msg_ret);
 484 }
 485
 486 static struct atomisp_sub_device *
 487 __get_asd_from_port(struct atomisp_device *isp, mipi_port_ID_t port)
 488 {
 489         int i;
 490
 491         /* Check which isp subdev to send eof */
 492         for (i = 0; i < isp->num_of_streams; i++) {
 493                 struct atomisp_sub_device *asd = &isp->asd[i];
 494                 struct camera_mipi_info *mipi_info;
 495
 496                 mipi_info = atomisp_to_sensor_mipi_info(
 497                                 isp->inputs[asd->input_curr].camera);
 498
 499                 if (asd->streaming == ATOMISP_DEVICE_STREAMING_ENABLED &&
 500                     __get_mipi_port(isp, mipi_info->port) == port) {
 501                         return asd;
 502                 }
 503         }
 504
 505         return NULL;
 506 }
 507
 508 /* interrupt handling function*/
 509 irqreturn_t atomisp_isr(int irq, void *dev)
 510 {
 511         struct atomisp_device *isp = (struct atomisp_device *)dev;
 512         struct atomisp_sub_device *asd;
 513         struct atomisp_css_event eof_event;
 514         unsigned int irq_infos = 0;
 515         unsigned long flags;
 516         unsigned int i;
 517         int err;
 518
 519         spin_lock_irqsave(&isp->lock, flags);
 520         if (isp->sw_contex.power_state != ATOM_ISP_POWER_UP ||
 521             isp->css_initialized == false) {
 522                 spin_unlock_irqrestore(&isp->lock, flags);
 523                 return IRQ_HANDLED;
 524         }
 525         err = atomisp_css_irq_translate(isp, &irq_infos);
 526         if (err) {
 527                 spin_unlock_irqrestore(&isp->lock, flags);
 528                 return IRQ_NONE;
 529         }
 530
 531         dev_dbg(isp->dev, "irq:0x%x\n", irq_infos);
 532
 533         clear_irq_reg(isp);
 534
 535         if (!atomisp_streaming_count(isp) && !atomisp_is_acc_enabled(isp))
 536                 goto out_nowake;
 537
 538         for (i = 0; i < isp->num_of_streams; i++) {
 539                 asd = &isp->asd[i];
 540
 541                 if (asd->streaming != ATOMISP_DEVICE_STREAMING_ENABLED)
 542                         continue;
 543                 /*
 544                  * Current SOF only support one stream, so the SOF only valid
 545                  * either solely one stream is running
 546                  */
 547                 if (irq_infos & CSS_IRQ_INFO_CSS_RECEIVER_SOF) {
 548                         atomic_inc(&asd->sof_count);
 549                         atomisp_sof_event(asd);
 550
 551                         /* If sequence_temp and sequence are the same
 552                          * there where no frames lost so we can increase
 553                          * sequence_temp.
 554                          * If not then processing of frame is still in progress
 555                          * and driver needs to keep old sequence_temp value.
 556                          * NOTE: There is assumption here that ISP will not
 557                          * start processing next frame from sensor before old
 558                          * one is completely done. */
 559                         if (atomic_read(&asd->sequence) == atomic_read(
 560                                                 &asd->sequence_temp))
 561                                 atomic_set(&asd->sequence_temp,
 562                                                 atomic_read(&asd->sof_count));
 563                 }
 564                 if (irq_infos & CSS_IRQ_INFO_EVENTS_READY)
 565                         atomic_set(&asd->sequence,
 566                                    atomic_read(&asd->sequence_temp));
 567         }
 568
 569         if (irq_infos & CSS_IRQ_INFO_CSS_RECEIVER_SOF)
 570                 irq_infos &= ~CSS_IRQ_INFO_CSS_RECEIVER_SOF;
 571
 572         if ((irq_infos & CSS_IRQ_INFO_INPUT_SYSTEM_ERROR) ||
 573             (irq_infos & CSS_IRQ_INFO_IF_ERROR)) {
 574                 /* handle mipi receiver error */
 575                 u32 rx_infos;
 576                 enum ia_css_csi2_port port;
 577
 578                 for (port = IA_CSS_CSI2_PORT0; port <= IA_CSS_CSI2_PORT2;
 579                      port++) {
 580                         print_csi_rx_errors(port, isp);
 581                         atomisp_css_rx_get_irq_info(port, &rx_infos);
 582                         atomisp_css_rx_clear_irq_info(port, rx_infos);
 583                 }
 584         }
 585
 586         if (irq_infos & IA_CSS_IRQ_INFO_ISYS_EVENTS_READY) {
 587                 while (ia_css_dequeue_isys_event(&(eof_event.event)) ==
 588                        IA_CSS_SUCCESS) {
 589                         /* EOF Event does not have the css_pipe returned */
 590                         asd = __get_asd_from_port(isp, eof_event.event.port);
 591                         if (!asd) {
 592                                 dev_err(isp->dev, "%s:no subdev.event:%d",  __func__,
 593                                         eof_event.event.type);
 594                                 continue;
 595                         }
 596
 597                         atomisp_eof_event(asd, eof_event.event.exp_id);
 598                         dev_dbg(isp->dev, "%s EOF exp_id %d, asd %d\n",
 599                                 __func__, eof_event.event.exp_id, asd->index);
 600                 }
 601
 602                 irq_infos &= ~IA_CSS_IRQ_INFO_ISYS_EVENTS_READY;
 603                 if (irq_infos == 0)
 604                         goto out_nowake;
 605         }
 606
 607         spin_unlock_irqrestore(&isp->lock, flags);
 608
 609         return IRQ_WAKE_THREAD;
 610
 611 out_nowake:
 612         spin_unlock_irqrestore(&isp->lock, flags);
 613
 614         return IRQ_HANDLED;
 615 }
 616
 617 void atomisp_clear_css_buffer_counters(struct atomisp_sub_device *asd)
 618 {
 619         int i;
 620         memset(asd->s3a_bufs_in_css, 0, sizeof(asd->s3a_bufs_in_css));
 621         for (i = 0; i < ATOMISP_INPUT_STREAM_NUM; i++)
 622                 memset(asd->metadata_bufs_in_css[i], 0,
 623                        sizeof(asd->metadata_bufs_in_css[i]));
 624         asd->dis_bufs_in_css = 0;
 625         asd->video_out_capture.buffers_in_css = 0;
 626         asd->video_out_vf.buffers_in_css = 0;
 627         asd->video_out_preview.buffers_in_css = 0;
 628         asd->video_out_video_capture.buffers_in_css = 0;
 629 }
 630
 631 #ifndef ISP2401
 632 bool atomisp_buffers_queued(struct atomisp_sub_device *asd)
 633 #else
 634 bool atomisp_buffers_queued_pipe(struct atomisp_video_pipe *pipe)
 635 #endif
 636 {
 637 #ifndef ISP2401
 638         return asd->video_out_capture.buffers_in_css ||
 639                 asd->video_out_vf.buffers_in_css ||
 640                 asd->video_out_preview.buffers_in_css ||
 641                 asd->video_out_video_capture.buffers_in_css ?
 642                     true : false;
 643 #else
 644         return pipe->buffers_in_css ? true : false;
 645 #endif
 646 }
 647
 648 /* 0x100000 is the start of dmem inside SP */
 649 #define SP_DMEM_BASE    0x100000
 650
 651 void dump_sp_dmem(struct atomisp_device *isp, unsigned int addr,
 652                   unsigned int size)
 653 {
 654         unsigned int data = 0;
 655         unsigned int size32 = DIV_ROUND_UP(size, sizeof(u32));
 656
 657         dev_dbg(isp->dev, "atomisp_io_base:%p\n", atomisp_io_base);
 658         dev_dbg(isp->dev, "%s, addr:0x%x, size: %d, size32: %d\n", __func__,
 659                         addr, size, size32);
 660         if (size32 * 4 + addr > 0x4000) {
 661                 dev_err(isp->dev, "illegal size (%d) or addr (0x%x)\n",
 662                                 size32, addr);
 663                 return;
 664         }
 665         addr += SP_DMEM_BASE;
 666         do {
 667                 data = _hrt_master_port_uload_32(addr);
 668
 669                 dev_dbg(isp->dev, "%s, \t [0x%x]:0x%x\n", __func__, addr, data);
 670                 addr += sizeof(unsigned int);
 671                 size32 -= 1;
 672         } while (size32 > 0);
 673 }
 674
 675 static struct videobuf_buffer *atomisp_css_frame_to_vbuf(
 676         struct atomisp_video_pipe *pipe, struct atomisp_css_frame *frame)
 677 {
 678         struct videobuf_vmalloc_memory *vm_mem;
 679         struct atomisp_css_frame *handle;
 680         int i;
 681
 682         for (i = 0; pipe->capq.bufs[i]; i++) {
 683                 vm_mem = pipe->capq.bufs[i]->priv;
 684                 handle = vm_mem->vaddr;
 685                 if (handle && handle->data == frame->data)
 686                         return pipe->capq.bufs[i];
 687         }
 688
 689         return NULL;
 690 }
 691
 692 static void get_buf_timestamp(struct timeval *tv)
 693 {
 694         struct timespec ts;
 695         ktime_get_ts(&ts);
 696         tv->tv_sec = ts.tv_sec;
 697         tv->tv_usec = ts.tv_nsec / NSEC_PER_USEC;
 698 }
 699
 700 static void atomisp_flush_video_pipe(struct atomisp_sub_device *asd,
 701                                      struct atomisp_video_pipe *pipe)
 702 {
 703         unsigned long irqflags;
 704         int i;
 705
 706         if (!pipe->users)
 707                 return;
 708
 709         for (i = 0; pipe->capq.bufs[i]; i++) {
 710                 spin_lock_irqsave(&pipe->irq_lock, irqflags);
 711                 if (pipe->capq.bufs[i]->state == VIDEOBUF_ACTIVE ||
 712                     pipe->capq.bufs[i]->state == VIDEOBUF_QUEUED) {
 713                         get_buf_timestamp(&pipe->capq.bufs[i]->ts);
 714                         pipe->capq.bufs[i]->field_count =
 715                                 atomic_read(&asd->sequence) << 1;
 716                         dev_dbg(asd->isp->dev, "release buffers on device %s\n",
 717                                 pipe->vdev.name);
 718                         if (pipe->capq.bufs[i]->state == VIDEOBUF_QUEUED)
 719                                 list_del_init(&pipe->capq.bufs[i]->queue);
 720                         pipe->capq.bufs[i]->state = VIDEOBUF_ERROR;
 721                         wake_up(&pipe->capq.bufs[i]->done);
 722                 }
 723                 spin_unlock_irqrestore(&pipe->irq_lock, irqflags);
 724         }
 725 }
 726
 727 /* Returns queued buffers back to video-core */
 728 void atomisp_flush_bufs_and_wakeup(struct atomisp_sub_device *asd)
 729 {
 730         atomisp_flush_video_pipe(asd, &asd->video_out_capture);
 731         atomisp_flush_video_pipe(asd, &asd->video_out_vf);
 732         atomisp_flush_video_pipe(asd, &asd->video_out_preview);
 733         atomisp_flush_video_pipe(asd, &asd->video_out_video_capture);
 734 }
 735
 736 /* clean out the parameters that did not apply */
 737 void atomisp_flush_params_queue(struct atomisp_video_pipe *pipe)
 738 {
 739         struct atomisp_css_params_with_list *param;
 740
 741         while (!list_empty(&pipe->per_frame_params)) {
 742                 param = list_entry(pipe->per_frame_params.next,
 743                                    struct atomisp_css_params_with_list, list);
 744                 list_del(&param->list);
 745                 atomisp_free_css_parameters(&param->params);
 746                 kvfree(param);
 747         }
 748 }
 749
 750 /* Re-queue per-frame parameters */
 751 static void atomisp_recover_params_queue(struct atomisp_video_pipe *pipe)
 752 {
 753         struct atomisp_css_params_with_list *param;
 754         int i;
 755
 756         for (i = 0; i < VIDEO_MAX_FRAME; i++) {
 757                 param = pipe->frame_params[i];
 758                 if (param)
 759                         list_add_tail(&param->list, &pipe->per_frame_params);
 760                 pipe->frame_params[i] = NULL;
 761         }
 762         atomisp_handle_parameter_and_buffer(pipe);
 763 }
 764
 765 /* find atomisp_video_pipe with css pipe id, buffer type and atomisp run_mode */
 766 static struct atomisp_video_pipe *__atomisp_get_pipe(
 767                 struct atomisp_sub_device *asd,
 768                 enum atomisp_input_stream_id stream_id,
 769                 enum atomisp_css_pipe_id css_pipe_id,
 770                 enum atomisp_css_buffer_type buf_type)
 771 {
 772         struct atomisp_device *isp = asd->isp;
 773
 774         if (css_pipe_id == CSS_PIPE_ID_COPY &&
 775             isp->inputs[asd->input_curr].camera_caps->
 776                 sensor[asd->sensor_curr].stream_num > 1) {
 777                 switch (stream_id) {
 778                 case ATOMISP_INPUT_STREAM_PREVIEW:
 779                         return &asd->video_out_preview;
 780                 case ATOMISP_INPUT_STREAM_POSTVIEW:
 781                         return &asd->video_out_vf;
 782                 case ATOMISP_INPUT_STREAM_VIDEO:
 783                         return &asd->video_out_video_capture;
 784                 case ATOMISP_INPUT_STREAM_CAPTURE:
 785                 default:
 786                         return &asd->video_out_capture;
 787                 }
 788         }
 789
 790         /* video is same in online as in continuouscapture mode */
 791         if (asd->vfpp->val == ATOMISP_VFPP_DISABLE_LOWLAT) {
 792                 /*
 793                  * Disable vf_pp and run CSS in still capture mode. In this
 794                  * mode, CSS does not cause extra latency with buffering, but
 795                  * scaling is not available.
 796                  */
 797                 return &asd->video_out_capture;
 798         } else if (asd->vfpp->val == ATOMISP_VFPP_DISABLE_SCALER) {
 799                 /*
 800                  * Disable vf_pp and run CSS in video mode. This allows using
 801                  * ISP scaling but it has one frame delay due to CSS internal
 802                  * buffering.
 803                  */
 804                 return &asd->video_out_video_capture;
 805         } else if (css_pipe_id == CSS_PIPE_ID_YUVPP) {
 806                 /*
 807                  * to SOC camera, yuvpp pipe is run for capture/video/SDV/ZSL.
 808                  */
 809                 if (asd->continuous_mode->val) {
 810                         if (asd->run_mode->val == ATOMISP_RUN_MODE_VIDEO) {
 811                                 /* SDV case */
 812                                 switch (buf_type) {
 813                                 case CSS_BUFFER_TYPE_SEC_OUTPUT_FRAME:
 814                                         return &asd->video_out_video_capture;
 815                                 case CSS_BUFFER_TYPE_SEC_VF_OUTPUT_FRAME:
 816                                         return &asd->video_out_preview;
 817                                 case CSS_BUFFER_TYPE_OUTPUT_FRAME:
 818                                         return &asd->video_out_capture;
 819                                 default:
 820                                         return &asd->video_out_vf;
 821                                 }
 822                         } else if (asd->run_mode->val == ATOMISP_RUN_MODE_PREVIEW) {
 823                                 /* ZSL case */
 824                                 switch (buf_type) {
 825                                 case CSS_BUFFER_TYPE_SEC_OUTPUT_FRAME:
 826                                         return &asd->video_out_preview;
 827                                 case CSS_BUFFER_TYPE_OUTPUT_FRAME:
 828                                         return &asd->video_out_capture;
 829                                 default:
 830                                         return &asd->video_out_vf;
 831                                 }
 832                         }
 833                 } else if (buf_type == CSS_BUFFER_TYPE_OUTPUT_FRAME) {
 834                         switch (asd->run_mode->val) {
 835                         case ATOMISP_RUN_MODE_VIDEO:
 836                                 return &asd->video_out_video_capture;
 837                         case ATOMISP_RUN_MODE_PREVIEW:
 838                                 return &asd->video_out_preview;
 839                         default:
 840                                 return &asd->video_out_capture;
 841                         }
 842                 } else if (buf_type == CSS_BUFFER_TYPE_VF_OUTPUT_FRAME) {
 843                         if (asd->run_mode->val == ATOMISP_RUN_MODE_VIDEO)
 844                                 return &asd->video_out_preview;
 845                         else
 846                                 return &asd->video_out_vf;
 847                 }
 848         } else if (asd->run_mode->val == ATOMISP_RUN_MODE_VIDEO) {
 849                 /* For online video or SDV video pipe. */
 850                 if (css_pipe_id == CSS_PIPE_ID_VIDEO ||
 851                     css_pipe_id == CSS_PIPE_ID_COPY) {
 852                         if (buf_type == CSS_BUFFER_TYPE_OUTPUT_FRAME)
 853                                 return &asd->video_out_video_capture;
 854                         return &asd->video_out_preview;
 855                 }
 856         } else if (asd->run_mode->val == ATOMISP_RUN_MODE_PREVIEW) {
 857                 /* For online preview or ZSL preview pipe. */
 858                 if (css_pipe_id == CSS_PIPE_ID_PREVIEW ||
 859                     css_pipe_id == CSS_PIPE_ID_COPY)
 860                         return &asd->video_out_preview;
 861         }
 862         /* For capture pipe. */
 863         if (buf_type == CSS_BUFFER_TYPE_VF_OUTPUT_FRAME)
 864                 return &asd->video_out_vf;
 865         return &asd->video_out_capture;
 866 }
 867
 868 enum atomisp_metadata_type
 869 atomisp_get_metadata_type(struct atomisp_sub_device *asd,
 870                           enum ia_css_pipe_id pipe_id)
 871 {
 872         if (!asd->continuous_mode->val)
 873                 return ATOMISP_MAIN_METADATA;
 874
 875         if (pipe_id == IA_CSS_PIPE_ID_CAPTURE) /* online capture pipe */
 876                 return ATOMISP_SEC_METADATA;
 877         else
 878                 return ATOMISP_MAIN_METADATA;
 879 }
 880
 881 void atomisp_buf_done(struct atomisp_sub_device *asd, int error,
 882                       enum atomisp_css_buffer_type buf_type,
 883                       enum atomisp_css_pipe_id css_pipe_id,
 884                       bool q_buffers, enum atomisp_input_stream_id stream_id)
 885 {
 886         struct videobuf_buffer *vb = NULL;
 887         struct atomisp_video_pipe *pipe = NULL;
 888         struct atomisp_css_buffer buffer;
 889         bool requeue = false;
 890         int err;
 891         unsigned long irqflags;
 892         struct atomisp_css_frame *frame = NULL;
 893         struct atomisp_s3a_buf *s3a_buf = NULL, *_s3a_buf_tmp;
 894         struct atomisp_dis_buf *dis_buf = NULL, *_dis_buf_tmp;
 895         struct atomisp_metadata_buf *md_buf = NULL, *_md_buf_tmp;
 896         enum atomisp_metadata_type md_type;
 897         struct atomisp_device *isp = asd->isp;
 898         struct v4l2_control ctrl;
 899 #ifdef ISP2401
 900         bool reset_wdt_timer = false;
 901 #endif
 902
 903         if (
 904             buf_type != CSS_BUFFER_TYPE_METADATA &&
 905             buf_type != CSS_BUFFER_TYPE_3A_STATISTICS &&
 906             buf_type != CSS_BUFFER_TYPE_DIS_STATISTICS &&
 907             buf_type != CSS_BUFFER_TYPE_OUTPUT_FRAME &&
 908             buf_type != CSS_BUFFER_TYPE_SEC_OUTPUT_FRAME &&
 909             buf_type != CSS_BUFFER_TYPE_RAW_OUTPUT_FRAME &&
 910             buf_type != CSS_BUFFER_TYPE_SEC_VF_OUTPUT_FRAME &&
 911             buf_type != CSS_BUFFER_TYPE_VF_OUTPUT_FRAME) {
 912                 dev_err(isp->dev, "%s, unsupported buffer type: %d\n",
 913                         __func__, buf_type);
 914                 return;
 915         }
 916
 917         memset(&buffer, 0, sizeof(struct atomisp_css_buffer));
 918         buffer.css_buffer.type = buf_type;
 919         err = atomisp_css_dequeue_buffer(asd, stream_id, css_pipe_id,
 920                                          buf_type, &buffer);
 921         if (err) {
 922                 dev_err(isp->dev,
 923                         "atomisp_css_dequeue_buffer failed: 0x%x\n", err);
 924                 return;
 925         }
 926
 927         /* need to know the atomisp pipe for frame buffers */
 928         pipe = __atomisp_get_pipe(asd, stream_id, css_pipe_id, buf_type);
 929         if (pipe == NULL) {
 930                 dev_err(isp->dev, "error getting atomisp pipe\n");
 931                 return;
 932         }
 933
 934         switch (buf_type) {
 935         case CSS_BUFFER_TYPE_3A_STATISTICS:
 936                 list_for_each_entry_safe(s3a_buf, _s3a_buf_tmp,
 937                                          &asd->s3a_stats_in_css, list) {
 938                         if (s3a_buf->s3a_data ==
 939                                 buffer.css_buffer.data.stats_3a) {
 940                                 list_del_init(&s3a_buf->list);
 941                                 list_add_tail(&s3a_buf->list,
 942                                               &asd->s3a_stats_ready);
 943                                 break;
 944                         }
 945                 }
 946
 947                 asd->s3a_bufs_in_css[css_pipe_id]--;
 948                 atomisp_3a_stats_ready_event(asd, buffer.css_buffer.exp_id);
 949                 dev_dbg(isp->dev, "%s: s3a stat with exp_id %d is ready\n",
 950                         __func__, s3a_buf->s3a_data->exp_id);
 951                 break;
 952         case CSS_BUFFER_TYPE_METADATA:
 953                 if (error)
 954                         break;
 955
 956                 md_type = atomisp_get_metadata_type(asd, css_pipe_id);
 957                 list_for_each_entry_safe(md_buf, _md_buf_tmp,
 958                                          &asd->metadata_in_css[md_type], list) {
 959                         if (md_buf->metadata ==
 960                                 buffer.css_buffer.data.metadata) {
 961                                 list_del_init(&md_buf->list);
 962                                 list_add_tail(&md_buf->list,
 963                                               &asd->metadata_ready[md_type]);
 964                                 break;
 965                         }
 966                 }
 967                 asd->metadata_bufs_in_css[stream_id][css_pipe_id]--;
 968                 atomisp_metadata_ready_event(asd, md_type);
 969                 dev_dbg(isp->dev, "%s: metadata with exp_id %d is ready\n",
 970                         __func__, md_buf->metadata->exp_id);
 971                 break;
 972         case CSS_BUFFER_TYPE_DIS_STATISTICS:
 973                 list_for_each_entry_safe(dis_buf, _dis_buf_tmp,
 974                                                 &asd->dis_stats_in_css, list) {
 975                         if (dis_buf->dis_data ==
 976                                 buffer.css_buffer.data.stats_dvs) {
 977                                 spin_lock_irqsave(&asd->dis_stats_lock,
 978                                                   irqflags);
 979                                 list_del_init(&dis_buf->list);
 980                                 list_add(&dis_buf->list, &asd->dis_stats);
 981                                 asd->params.dis_proj_data_valid = true;
 982                                 spin_unlock_irqrestore(&asd->dis_stats_lock,
 983                                                        irqflags);
 984                                 break;
 985                         }
 986                 }
 987                 asd->dis_bufs_in_css--;
 988                 dev_dbg(isp->dev, "%s: dis stat with exp_id %d is ready\n",
 989                         __func__, dis_buf->dis_data->exp_id);
 990                 break;
 991         case CSS_BUFFER_TYPE_VF_OUTPUT_FRAME:
 992         case CSS_BUFFER_TYPE_SEC_VF_OUTPUT_FRAME:
 993 #ifdef ISP2401
 994                 reset_wdt_timer = true;
 995 #endif
 996                 pipe->buffers_in_css--;
 997                 frame = buffer.css_buffer.data.frame;
 998                 if (!frame) {
 999                         WARN_ON(1);
1000                         break;
1001                 }
1002                 if (!frame->valid)
1003                         error = true;
1004
1005                 /* FIXME:
1006                  * YUVPP doesn't set postview exp_id correctlly in SDV mode.
1007                  * This is a WORKAROUND to set exp_id. see HSDES-1503911606.
1008                  */
1009                 if (IS_BYT && buf_type == CSS_BUFFER_TYPE_SEC_VF_OUTPUT_FRAME &&
1010                     asd->continuous_mode->val && ATOMISP_USE_YUVPP(asd))
1011                         frame->exp_id = (asd->postview_exp_id++) %
1012                                                 (ATOMISP_MAX_EXP_ID + 1);
1013
1014                 dev_dbg(isp->dev, "%s: vf frame with exp_id %d is ready\n",
1015                         __func__, frame->exp_id);
1016                 if (asd->params.flash_state == ATOMISP_FLASH_ONGOING) {
1017                         if (frame->flash_state
1018                             == CSS_FRAME_FLASH_STATE_PARTIAL)
1019                                 dev_dbg(isp->dev, "%s thumb partially flashed\n",
1020                                         __func__);
1021                         else if (frame->flash_state
1022                                  == CSS_FRAME_FLASH_STATE_FULL)
1023                                 dev_dbg(isp->dev, "%s thumb completely flashed\n",
1024                                         __func__);
1025                         else
1026                                 dev_dbg(isp->dev, "%s thumb no flash in this frame\n",
1027                                         __func__);
1028                 }
1029                 vb = atomisp_css_frame_to_vbuf(pipe, frame);
1030                 WARN_ON(!vb);
1031                 if (vb)
1032                         pipe->frame_config_id[vb->i] = frame->isp_config_id;
1033                 if (css_pipe_id == IA_CSS_PIPE_ID_CAPTURE &&
1034                     asd->pending_capture_request > 0) {
1035                         err = atomisp_css_offline_capture_configure(asd,
1036                                         asd->params.offline_parm.num_captures,
1037                                         asd->params.offline_parm.skip_frames,
1038                                         asd->params.offline_parm.offset);
1039 #ifndef ISP2401
1040                         asd->pending_capture_request--;
1041                         dev_dbg(isp->dev, "Trigger capture again for new buffer. err=%d\n",
1042                                 err);
1043 #else
1044                                 asd->pending_capture_request--;
1045                                 asd->re_trigger_capture = false;
1046                                 dev_dbg(isp->dev, "Trigger capture again for new buffer. err=%d\n",
1047                                                 err);
1048                         } else {
1049                                 asd->re_trigger_capture = true;
1050                         }
1051 #endif
1052                 }
1053                 break;
1054         case CSS_BUFFER_TYPE_OUTPUT_FRAME:
1055         case CSS_BUFFER_TYPE_SEC_OUTPUT_FRAME:
1056 #ifdef ISP2401
1057                 reset_wdt_timer = true;
1058 #endif
1059                 pipe->buffers_in_css--;
1060                 frame = buffer.css_buffer.data.frame;
1061                 if (!frame) {
1062                         WARN_ON(1);
1063                         break;
1064                 }
1065
1066                 if (!frame->valid)
1067                         error = true;
1068
1069                 /* FIXME:
1070                  * YUVPP doesn't set preview exp_id correctlly in ZSL mode.
1071                  * This is a WORKAROUND to set exp_id. see HSDES-1503911606.
1072                  */
1073                 if (IS_BYT && buf_type == CSS_BUFFER_TYPE_SEC_OUTPUT_FRAME &&
1074                     asd->continuous_mode->val && ATOMISP_USE_YUVPP(asd))
1075                         frame->exp_id = (asd->preview_exp_id++) %
1076                                                 (ATOMISP_MAX_EXP_ID + 1);
1077
1078                 dev_dbg(isp->dev, "%s: main frame with exp_id %d is ready\n",
1079                         __func__, frame->exp_id);
1080                 vb = atomisp_css_frame_to_vbuf(pipe, frame);
1081                 if (!vb) {
1082                         WARN_ON(1);
1083                         break;
1084                 }
1085
1086                 /* free the parameters */
1087                 if (pipe->frame_params[vb->i]) {
1088                         if (asd->params.dvs_6axis ==
1089                             pipe->frame_params[vb->i]->params.dvs_6axis)
1090                                 asd->params.dvs_6axis = NULL;
1091                         atomisp_free_css_parameters(
1092                                 &pipe->frame_params[vb->i]->params);
1093                         kvfree(pipe->frame_params[vb->i]);
1094                         pipe->frame_params[vb->i] = NULL;
1095                 }
1096
1097                 pipe->frame_config_id[vb->i] = frame->isp_config_id;
1098                 ctrl.id = V4L2_CID_FLASH_MODE;
1099                 if (asd->params.flash_state == ATOMISP_FLASH_ONGOING) {
1100                         if (frame->flash_state
1101                             == CSS_FRAME_FLASH_STATE_PARTIAL) {
1102                                 asd->frame_status[vb->i] =
1103                                         ATOMISP_FRAME_STATUS_FLASH_PARTIAL;
1104                                 dev_dbg(isp->dev, "%s partially flashed\n",
1105                                          __func__);
1106                         } else if (frame->flash_state
1107                                    == CSS_FRAME_FLASH_STATE_FULL) {
1108                                 asd->frame_status[vb->i] =
1109                                         ATOMISP_FRAME_STATUS_FLASH_EXPOSED;
1110                                 asd->params.num_flash_frames--;
1111                                 dev_dbg(isp->dev, "%s completely flashed\n",
1112                                          __func__);
1113                         } else {
1114                                 asd->frame_status[vb->i] =
1115                                         ATOMISP_FRAME_STATUS_OK;
1116                                 dev_dbg(isp->dev,
1117                                          "%s no flash in this frame\n",
1118                                          __func__);
1119                         }
1120
1121                         /* Check if flashing sequence is done */
1122                         if (asd->frame_status[vb->i] ==
1123                                 ATOMISP_FRAME_STATUS_FLASH_EXPOSED)
1124                                 asd->params.flash_state = ATOMISP_FLASH_DONE;
1125                 } else if (isp->flash) {
1126                         if (v4l2_g_ctrl(isp->flash->ctrl_handler, &ctrl) ==
1127                             0 && ctrl.value == ATOMISP_FLASH_MODE_TORCH) {
1128                                 ctrl.id = V4L2_CID_FLASH_TORCH_INTENSITY;
1129                                 if (v4l2_g_ctrl(isp->flash->ctrl_handler, &ctrl)
1130                                     == 0 && ctrl.value > 0) {
1131                                         asd->frame_status[vb->i] =
1132                                             ATOMISP_FRAME_STATUS_FLASH_EXPOSED;
1133                                 } else {
1134                                         asd->frame_status[vb->i] =
1135                                             ATOMISP_FRAME_STATUS_OK;
1136                                 }
1137                         } else
1138                                 asd->frame_status[vb->i] =
1139                                     ATOMISP_FRAME_STATUS_OK;
1140                 } else {
1141                         asd->frame_status[vb->i] = ATOMISP_FRAME_STATUS_OK;
1142                 }
1143
1144                 asd->params.last_frame_status = asd->frame_status[vb->i];
1145
1146                 if (asd->continuous_mode->val) {
1147                         if (css_pipe_id == CSS_PIPE_ID_PREVIEW ||
1148                             css_pipe_id == CSS_PIPE_ID_VIDEO) {
1149                                 asd->latest_preview_exp_id = frame->exp_id;
1150                         } else if (css_pipe_id ==
1151                                         CSS_PIPE_ID_CAPTURE) {
1152                                 if (asd->run_mode->val ==
1153                                         ATOMISP_RUN_MODE_VIDEO)
1154                                         dev_dbg(isp->dev, "SDV capture raw buffer id: %u\n",
1155                                             frame->exp_id);
1156                                 else
1157                                         dev_dbg(isp->dev, "ZSL capture raw buffer id: %u\n",
1158                                             frame->exp_id);
1159                         }
1160                 }
1161                 /*
1162                  * Only after enabled the raw buffer lock
1163                  * and in continuous mode.
1164                  * in preview/video pipe, each buffer will
1165                  * be locked automatically, so record it here.
1166                  */
1167                 if (((css_pipe_id == CSS_PIPE_ID_PREVIEW) ||
1168                     (css_pipe_id == CSS_PIPE_ID_VIDEO)) &&
1169                     asd->enable_raw_buffer_lock->val &&
1170                     asd->continuous_mode->val) {
1171                         atomisp_set_raw_buffer_bitmap(asd, frame->exp_id);
1172                         WARN_ON(frame->exp_id > ATOMISP_MAX_EXP_ID);
1173                 }
1174
1175                 if (asd->params.css_update_params_needed) {
1176                         atomisp_apply_css_parameters(asd,
1177                                                      &asd->params.css_param);
1178                         if (asd->params.css_param.update_flag.dz_config)
1179                                 atomisp_css_set_dz_config(asd,
1180                                         &asd->params.css_param.dz_config);
1181                         /* New global dvs 6axis config should be blocked
1182                          * here if there's a buffer with per-frame parameters
1183                          * pending in CSS frame buffer queue.
1184                          * This is to aviod zooming vibration since global
1185                          * parameters take effect immediately while
1186                          * per-frame parameters are taken after previous
1187                          * buffers in CSS got processed.
1188                          */
1189                         if (asd->params.dvs_6axis)
1190                                 atomisp_css_set_dvs_6axis(asd,
1191                                         asd->params.dvs_6axis);
1192                         else
1193                                 asd->params.css_update_params_needed = false;
1194                         /* The update flag should not be cleaned here
1195                          * since it is still going to be used to make up
1196                          * following per-frame parameters.
1197                          * This will introduce more copy work since each
1198                          * time when updating global parameters, the whole
1199                          * parameter set are applied.
1200                          * FIXME: A new set of parameter copy functions can
1201                          * be added to make up per-frame parameters based on
1202                          * solid structures stored in asd->params.css_param
1203                          * instead of using shadow pointers in update flag.
1204                          */
1205                         atomisp_css_update_isp_params(asd);
1206                 }
1207                 break;
1208         default:
1209                 break;
1210         }
1211         if (vb) {
1212                 get_buf_timestamp(&vb->ts);
1213                 vb->field_count = atomic_read(&asd->sequence) << 1;
1214                 /*mark videobuffer done for dequeue*/
1215                 spin_lock_irqsave(&pipe->irq_lock, irqflags);
1216                 vb->state = !error ? VIDEOBUF_DONE : VIDEOBUF_ERROR;
1217                 spin_unlock_irqrestore(&pipe->irq_lock, irqflags);
1218
1219                 /*
1220                  * Frame capture done, wake up any process block on
1221                  * current active buffer
1222                  * possibly hold by videobuf_dqbuf()
1223                  */
1224                 wake_up(&vb->done);
1225         }
1226 #ifdef ISP2401
1227         atomic_set(&pipe->wdt_count, 0);
1228 #endif
1229         /*
1230          * Requeue should only be done for 3a and dis buffers.
1231          * Queue/dequeue order will change if driver recycles image buffers.
1232          */
1233         if (requeue) {
1234                 err = atomisp_css_queue_buffer(asd,
1235                                                stream_id, css_pipe_id,
1236                                                buf_type, &buffer);
1237                 if (err)
1238                         dev_err(isp->dev, "%s, q to css fails: %d\n",
1239                                         __func__, err);
1240                 return;
1241         }
1242         if (!error && q_buffers)
1243                 atomisp_qbuffers_to_css(asd);
1244 #ifdef ISP2401
1245
1246         /* If there are no buffers queued then
1247          * delete wdt timer. */
1248         if (asd->streaming != ATOMISP_DEVICE_STREAMING_ENABLED)
1249                 return;
1250         if (!atomisp_buffers_queued_pipe(pipe))
1251                 atomisp_wdt_stop_pipe(pipe, false);
1252         else if (reset_wdt_timer)
1253                 /* SOF irq should not reset wdt timer. */
1254                 atomisp_wdt_refresh_pipe(pipe,
1255                                          ATOMISP_WDT_KEEP_CURRENT_DELAY);
1256 #endif
1257 }
1258
1259 void atomisp_delayed_init_work(struct work_struct *work)
1260 {
1261         struct atomisp_sub_device *asd = container_of(work,
1262                         struct atomisp_sub_device,
1263                         delayed_init_work);
1264         /*
1265          * to SOC camera, use yuvpp pipe and no support continuous mode.
1266          */
1267         if (!ATOMISP_USE_YUVPP(asd)) {
1268                 struct v4l2_event event = {0};
1269
1270                 atomisp_css_allocate_continuous_frames(false, asd);
1271                 atomisp_css_update_continuous_frames(asd);
1272
1273                 event.type = V4L2_EVENT_ATOMISP_RAW_BUFFERS_ALLOC_DONE;
1274                 v4l2_event_queue(asd->subdev.devnode, &event);
1275         }
1276
1277         /* signal streamon after delayed init is done */
1278         asd->delayed_init = ATOMISP_DELAYED_INIT_DONE;
1279         complete(&asd->init_done);
1280 }
1281
1282 static void __atomisp_css_recover(struct atomisp_device *isp, bool isp_timeout)
1283 {
1284         enum atomisp_css_pipe_id css_pipe_id;
1285         bool stream_restart[MAX_STREAM_NUM] = {0};
1286         bool depth_mode = false;
1287         int i, ret, depth_cnt = 0;
1288
1289         if (!isp->sw_contex.file_input)
1290                 atomisp_css_irq_enable(isp,
1291                                        CSS_IRQ_INFO_CSS_RECEIVER_SOF, false);
1292
1293         BUG_ON(isp->num_of_streams > MAX_STREAM_NUM);
1294
1295         for (i = 0; i < isp->num_of_streams; i++) {
1296                 struct atomisp_sub_device *asd = &isp->asd[i];
1297                 struct ia_css_pipeline *acc_pipeline;
1298                 struct ia_css_pipe *acc_pipe = NULL;
1299
1300                 if (asd->streaming != ATOMISP_DEVICE_STREAMING_ENABLED &&
1301                     !asd->stream_prepared)
1302                         continue;
1303
1304                 /*
1305                 * AtomISP::waitStageUpdate is blocked when WDT happens.
1306                 * By calling acc_done() for all loaded fw_handles,
1307                 * HAL will be unblocked.
1308                 */
1309                 acc_pipe = asd->stream_env[i].pipes[CSS_PIPE_ID_ACC];
1310                 if (acc_pipe != NULL) {
1311                         acc_pipeline = ia_css_pipe_get_pipeline(acc_pipe);
1312                         if (acc_pipeline) {
1313                                 struct ia_css_pipeline_stage *stage;
1314                                 for (stage = acc_pipeline->stages; stage;
1315                                         stage = stage->next) {
1316                                         const struct ia_css_fw_info *fw;
1317                                         fw = stage->firmware;
1318                                         atomisp_acc_done(asd, fw->handle);
1319                                 }
1320                         }
1321                 }
1322
1323                 depth_cnt++;
1324
1325                 if (asd->delayed_init == ATOMISP_DELAYED_INIT_QUEUED)
1326                         cancel_work_sync(&asd->delayed_init_work);
1327
1328                 complete(&asd->init_done);
1329                 asd->delayed_init = ATOMISP_DELAYED_INIT_NOT_QUEUED;
1330
1331                 stream_restart[asd->index] = true;
1332
1333                 asd->streaming = ATOMISP_DEVICE_STREAMING_STOPPING;
1334
1335                 /* stream off sensor */
1336                 ret = v4l2_subdev_call(
1337                                 isp->inputs[asd->input_curr].
1338                                 camera, video, s_stream, 0);
1339                 if (ret)
1340                         dev_warn(isp->dev,
1341                                         "can't stop streaming on sensor!\n");
1342
1343                 atomisp_acc_unload_extensions(asd);
1344
1345                 atomisp_clear_css_buffer_counters(asd);
1346
1347                 css_pipe_id = atomisp_get_css_pipe_id(asd);
1348                 atomisp_css_stop(asd, css_pipe_id, true);
1349
1350                 asd->streaming = ATOMISP_DEVICE_STREAMING_DISABLED;
1351
1352                 asd->preview_exp_id = 1;
1353                 asd->postview_exp_id = 1;
1354                 /* notify HAL the CSS reset */
1355                 dev_dbg(isp->dev,
1356                         "send reset event to %s\n", asd->subdev.devnode->name);
1357                 atomisp_reset_event(asd);
1358         }
1359
1360         /* clear irq */
1361         disable_isp_irq(hrt_isp_css_irq_sp);
1362         clear_isp_irq(hrt_isp_css_irq_sp);
1363
1364         /* Set the SRSE to 3 before resetting */
1365         pci_write_config_dword(isp->pdev, PCI_I_CONTROL, isp->saved_regs.i_control |
1366                                MRFLD_PCI_I_CONTROL_SRSE_RESET_MASK);
1367
1368         /* reset ISP and restore its state */
1369         isp->isp_timeout = true;
1370         atomisp_reset(isp);
1371         isp->isp_timeout = false;
1372
1373         if (!isp_timeout) {
1374                 for (i = 0; i < isp->num_of_streams; i++) {
1375                         if (isp->asd[i].depth_mode->val)
1376                                 return;
1377                 }
1378         }
1379
1380         for (i = 0; i < isp->num_of_streams; i++) {
1381                 struct atomisp_sub_device *asd = &isp->asd[i];
1382
1383                 if (!stream_restart[i])
1384                         continue;
1385
1386                 if (isp->inputs[asd->input_curr].type != FILE_INPUT)
1387                         atomisp_css_input_set_mode(asd,
1388                                         CSS_INPUT_MODE_SENSOR);
1389
1390                 css_pipe_id = atomisp_get_css_pipe_id(asd);
1391                 if (atomisp_css_start(asd, css_pipe_id, true))
1392                         dev_warn(isp->dev,
1393                                 "start SP failed, so do not set streaming to be enable!\n");
1394                 else
1395                         asd->streaming = ATOMISP_DEVICE_STREAMING_ENABLED;
1396
1397                 atomisp_csi2_configure(asd);
1398         }
1399
1400         if (!isp->sw_contex.file_input) {
1401                 atomisp_css_irq_enable(isp, CSS_IRQ_INFO_CSS_RECEIVER_SOF,
1402                                 atomisp_css_valid_sof(isp));
1403
1404                 if (atomisp_freq_scaling(isp, ATOMISP_DFS_MODE_AUTO, true) < 0)
1405                         dev_dbg(isp->dev, "dfs failed!\n");
1406         } else {
1407                 if (atomisp_freq_scaling(isp, ATOMISP_DFS_MODE_MAX, true) < 0)
1408                         dev_dbg(isp->dev, "dfs failed!\n");
1409         }
1410
1411         for (i = 0; i < isp->num_of_streams; i++) {
1412                 struct atomisp_sub_device *asd;
1413
1414                 asd = &isp->asd[i];
1415
1416                 if (!stream_restart[i])
1417                         continue;
1418
1419                 if (asd->continuous_mode->val &&
1420                     asd->delayed_init == ATOMISP_DELAYED_INIT_NOT_QUEUED) {
1421                         reinit_completion(&asd->init_done);
1422                         asd->delayed_init = ATOMISP_DELAYED_INIT_QUEUED;
1423                         queue_work(asd->delayed_init_workq,
1424                                         &asd->delayed_init_work);
1425                 }
1426                 /*
1427                  * dequeueing buffers is not needed. CSS will recycle
1428                  * buffers that it has.
1429                  */
1430                 atomisp_flush_bufs_and_wakeup(asd);
1431
1432                 /* Requeue unprocessed per-frame parameters. */
1433                 atomisp_recover_params_queue(&asd->video_out_capture);
1434                 atomisp_recover_params_queue(&asd->video_out_preview);
1435                 atomisp_recover_params_queue(&asd->video_out_video_capture);
1436
1437                 if ((asd->depth_mode->val) &&
1438                     (depth_cnt == ATOMISP_DEPTH_SENSOR_STREAMON_COUNT)) {
1439                         depth_mode = true;
1440                         continue;
1441                 }
1442
1443                 ret = v4l2_subdev_call(
1444                                 isp->inputs[asd->input_curr].camera, video,
1445                                 s_stream, 1);
1446                 if (ret)
1447                         dev_warn(isp->dev,
1448                                  "can't start streaming on sensor!\n");
1449
1450         }
1451
1452         if (depth_mode) {
1453                 if (atomisp_stream_on_master_slave_sensor(isp, true))
1454                         dev_warn(isp->dev,
1455                                  "master slave sensor stream on failed!\n");
1456         }
1457 }
1458
1459 void atomisp_wdt_work(struct work_struct *work)
1460 {
1461         struct atomisp_device *isp = container_of(work, struct atomisp_device,
1462                                                   wdt_work);
1463         int i;
1464 #ifdef ISP2401
1465         unsigned int pipe_wdt_cnt[MAX_STREAM_NUM][4] = { {0} };
1466         bool css_recover = true;
1467 #endif
1468
1469         rt_mutex_lock(&isp->mutex);
1470         if (!atomisp_streaming_count(isp)) {
1471                 atomic_set(&isp->wdt_work_queued, 0);
1472                 rt_mutex_unlock(&isp->mutex);
1473                 return;
1474         }
1475
1476 #ifndef ISP2401
1477         dev_err(isp->dev, "timeout %d of %d\n",
1478                 atomic_read(&isp->wdt_count) + 1,
1479                 ATOMISP_ISP_MAX_TIMEOUT_COUNT);
1480 #else
1481         for (i = 0; i < isp->num_of_streams; i++) {
1482                 struct atomisp_sub_device *asd = &isp->asd[i];
1483                 pipe_wdt_cnt[i][0] +=
1484                         atomic_read(&asd->video_out_capture.wdt_count);
1485                 pipe_wdt_cnt[i][1] +=
1486                         atomic_read(&asd->video_out_vf.wdt_count);
1487                 pipe_wdt_cnt[i][2] +=
1488                         atomic_read(&asd->video_out_preview.wdt_count);
1489                 pipe_wdt_cnt[i][3] +=
1490                         atomic_read(&asd->video_out_video_capture.wdt_count);
1491                 css_recover =
1492                         (pipe_wdt_cnt[i][0] <= ATOMISP_ISP_MAX_TIMEOUT_COUNT &&
1493                          pipe_wdt_cnt[i][1] <= ATOMISP_ISP_MAX_TIMEOUT_COUNT &&
1494                          pipe_wdt_cnt[i][2] <= ATOMISP_ISP_MAX_TIMEOUT_COUNT &&
1495                          pipe_wdt_cnt[i][3] <= ATOMISP_ISP_MAX_TIMEOUT_COUNT)
1496                          ? true : false;
1497                 dev_err(isp->dev, "pipe on asd%d timeout cnt: (%d, %d, %d, %d) of %d, recover = %d\n",
1498                         asd->index, pipe_wdt_cnt[i][0], pipe_wdt_cnt[i][1],
1499                         pipe_wdt_cnt[i][2], pipe_wdt_cnt[i][3],
1500                         ATOMISP_ISP_MAX_TIMEOUT_COUNT, css_recover);
1501         }
1502 #endif
1503
1504 #ifndef ISP2401
1505         if (atomic_inc_return(&isp->wdt_count) <
1506             ATOMISP_ISP_MAX_TIMEOUT_COUNT) {
1507 #else
1508         if (css_recover) {
1509 #endif
1510                 unsigned int old_dbglevel = dbg_level;
1511                 atomisp_css_debug_dump_sp_sw_debug_info();
1512                 atomisp_css_debug_dump_debug_info(__func__);
1513                 dbg_level = old_dbglevel;
1514                 for (i = 0; i < isp->num_of_streams; i++) {
1515                         struct atomisp_sub_device *asd = &isp->asd[i];
1516
1517                         if (asd->streaming != ATOMISP_DEVICE_STREAMING_ENABLED)
1518                                 continue;
1519                         dev_err(isp->dev, "%s, vdev %s buffers in css: %d\n",
1520                                 __func__,
1521                                 asd->video_out_capture.vdev.name,
1522                                 asd->video_out_capture.
1523                                 buffers_in_css);
1524                         dev_err(isp->dev,
1525                                 "%s, vdev %s buffers in css: %d\n",
1526                                 __func__,
1527                                 asd->video_out_vf.vdev.name,
1528                                 asd->video_out_vf.
1529                                 buffers_in_css);
1530                         dev_err(isp->dev,
1531                                 "%s, vdev %s buffers in css: %d\n",
1532                                 __func__,
1533                                 asd->video_out_preview.vdev.name,
1534                                 asd->video_out_preview.
1535                                 buffers_in_css);
1536                         dev_err(isp->dev,
1537                                 "%s, vdev %s buffers in css: %d\n",
1538                                 __func__,
1539                                 asd->video_out_video_capture.vdev.name,
1540                                 asd->video_out_video_capture.
1541                                 buffers_in_css);
1542                         dev_err(isp->dev,
1543                                 "%s, s3a buffers in css preview pipe:%d\n",
1544                                 __func__,
1545                                 asd->s3a_bufs_in_css[CSS_PIPE_ID_PREVIEW]);
1546                         dev_err(isp->dev,
1547                                 "%s, s3a buffers in css capture pipe:%d\n",
1548                                 __func__,
1549                                 asd->s3a_bufs_in_css[CSS_PIPE_ID_CAPTURE]);
1550                         dev_err(isp->dev,
1551                                 "%s, s3a buffers in css video pipe:%d\n",
1552                                 __func__,
1553                                 asd->s3a_bufs_in_css[CSS_PIPE_ID_VIDEO]);
1554                         dev_err(isp->dev,
1555                                 "%s, dis buffers in css: %d\n",
1556                                 __func__, asd->dis_bufs_in_css);
1557                         dev_err(isp->dev,
1558                                 "%s, metadata buffers in css preview pipe:%d\n",
1559                                 __func__,
1560                                 asd->metadata_bufs_in_css
1561                                 [ATOMISP_INPUT_STREAM_GENERAL]
1562                                 [CSS_PIPE_ID_PREVIEW]);
1563                         dev_err(isp->dev,
1564                                 "%s, metadata buffers in css capture pipe:%d\n",
1565                                 __func__,
1566                                 asd->metadata_bufs_in_css
1567                                 [ATOMISP_INPUT_STREAM_GENERAL]
1568                                 [CSS_PIPE_ID_CAPTURE]);
1569                         dev_err(isp->dev,
1570                                 "%s, metadata buffers in css video pipe:%d\n",
1571                                 __func__,
1572                                 asd->metadata_bufs_in_css
1573                                 [ATOMISP_INPUT_STREAM_GENERAL]
1574                                 [CSS_PIPE_ID_VIDEO]);
1575                         if (asd->enable_raw_buffer_lock->val) {
1576                                 unsigned int j;
1577
1578                                 dev_err(isp->dev, "%s, raw_buffer_locked_count %d\n",
1579                                         __func__, asd->raw_buffer_locked_count);
1580                                 for (j = 0; j <= ATOMISP_MAX_EXP_ID/32; j++)
1581                                         dev_err(isp->dev, "%s, raw_buffer_bitmap[%d]: 0x%x\n",
1582                                                 __func__, j,
1583                                                 asd->raw_buffer_bitmap[j]);
1584                         }
1585                 }
1586
1587                 /*sh_css_dump_sp_state();*/
1588                 /*sh_css_dump_isp_state();*/
1589         } else {
1590                 for (i = 0; i < isp->num_of_streams; i++) {
1591                         struct atomisp_sub_device *asd = &isp->asd[i];
1592                         if (asd->streaming ==
1593                             ATOMISP_DEVICE_STREAMING_ENABLED) {
1594                                 atomisp_clear_css_buffer_counters(asd);
1595                                 atomisp_flush_bufs_and_wakeup(asd);
1596                                 complete(&asd->init_done);
1597                         }
1598 #ifdef ISP2401
1599                         atomisp_wdt_stop(asd, false);
1600 #endif
1601                 }
1602
1603 #ifndef ISP2401
1604                 atomic_set(&isp->wdt_count, 0);
1605 #endif
1606                 isp->isp_fatal_error = true;
1607                 atomic_set(&isp->wdt_work_queued, 0);
1608
1609                 rt_mutex_unlock(&isp->mutex);
1610                 return;
1611         }
1612
1613         __atomisp_css_recover(isp, true);
1614 #ifdef ISP2401
1615         for (i = 0; i < isp->num_of_streams; i++) {
1616                 struct atomisp_sub_device *asd = &isp->asd[i];
1617                 if (asd->streaming ==
1618                         ATOMISP_DEVICE_STREAMING_ENABLED) {
1619                         atomisp_wdt_refresh(asd,
1620                                 isp->sw_contex.file_input ?
1621                                 ATOMISP_ISP_FILE_TIMEOUT_DURATION :
1622                                 ATOMISP_ISP_TIMEOUT_DURATION);
1623                 }
1624         }
1625 #endif
1626         dev_err(isp->dev, "timeout recovery handling done\n");
1627         atomic_set(&isp->wdt_work_queued, 0);
1628
1629         rt_mutex_unlock(&isp->mutex);
1630 }
1631
1632 void atomisp_css_flush(struct atomisp_device *isp)
1633 {
1634         int i;
1635
1636         if (!atomisp_streaming_count(isp))
1637                 return;
1638
1639         /* Disable wdt */
1640         for (i = 0; i < isp->num_of_streams; i++) {
1641                 struct atomisp_sub_device *asd = &isp->asd[i];
1642                 atomisp_wdt_stop(asd, true);
1643         }
1644
1645         /* Start recover */
1646         __atomisp_css_recover(isp, false);
1647         /* Restore wdt */
1648         for (i = 0; i < isp->num_of_streams; i++) {
1649                 struct atomisp_sub_device *asd = &isp->asd[i];
1650
1651                 if (asd->streaming !=
1652                                 ATOMISP_DEVICE_STREAMING_ENABLED)
1653                         continue;
1654
1655                 atomisp_wdt_refresh(asd,
1656                                     isp->sw_contex.file_input ?
1657                                     ATOMISP_ISP_FILE_TIMEOUT_DURATION :
1658                                     ATOMISP_ISP_TIMEOUT_DURATION);
1659         }
1660         dev_dbg(isp->dev, "atomisp css flush done\n");
1661 }
1662
1663 #ifndef ISP2401
1664 void atomisp_wdt(unsigned long isp_addr)
1665 #else
1666 void atomisp_wdt(unsigned long pipe_addr)
1667 #endif
1668 {
1669 #ifndef ISP2401
1670         struct atomisp_device *isp = (struct atomisp_device *)isp_addr;
1671 #else
1672         struct atomisp_video_pipe *pipe =
1673                 (struct atomisp_video_pipe *)pipe_addr;
1674         struct atomisp_sub_device *asd = pipe->asd;
1675         struct atomisp_device *isp = asd->isp;
1676 #endif
1677
1678 #ifdef ISP2401
1679         atomic_inc(&pipe->wdt_count);
1680         dev_warn(isp->dev,
1681                 "[WARNING]asd %d pipe %s ISP timeout %d!\n",
1682                         asd->index, pipe->vdev.name,
1683                         atomic_read(&pipe->wdt_count));
1684 #endif
1685         if (atomic_read(&isp->wdt_work_queued)) {
1686                 dev_dbg(isp->dev, "ISP watchdog was put into workqueue\n");
1687                 return;
1688         }
1689         atomic_set(&isp->wdt_work_queued, 1);
1690         queue_work(isp->wdt_work_queue, &isp->wdt_work);
1691 }
1692
1693 #ifndef ISP2401
1694 void atomisp_wdt_refresh(struct atomisp_sub_device *asd, unsigned int delay)
1695 #else
1696 void atomisp_wdt_refresh_pipe(struct atomisp_video_pipe *pipe,
1697                                 unsigned int delay)
1698 #endif
1699 {
1700         unsigned long next;
1701
1702         if (delay != ATOMISP_WDT_KEEP_CURRENT_DELAY)
1703 #ifndef ISP2401
1704                 asd->wdt_duration = delay;
1705 #else
1706                 pipe->wdt_duration = delay;
1707 #endif
1708
1709 #ifndef ISP2401
1710         next = jiffies + asd->wdt_duration;
1711 #else
1712         next = jiffies + pipe->wdt_duration;
1713 #endif
1714
1715         /* Override next if it has been pushed beyon the "next" time */
1716 #ifndef ISP2401
1717         if (atomisp_is_wdt_running(asd) && time_after(asd->wdt_expires, next))
1718                 next = asd->wdt_expires;
1719 #else
1720         if (atomisp_is_wdt_running(pipe) && time_after(pipe->wdt_expires, next))
1721                 next = pipe->wdt_expires;
1722 #endif
1723
1724 #ifndef ISP2401
1725         asd->wdt_expires = next;
1726 #else
1727         pipe->wdt_expires = next;
1728 #endif
1729
1730 #ifndef ISP2401
1731         if (atomisp_is_wdt_running(asd))
1732                 dev_dbg(asd->isp->dev, "WDT will hit after %d ms\n",
1733                         ((int)(next - jiffies) * 1000 / HZ));
1734 #else
1735         if (atomisp_is_wdt_running(pipe))
1736                 dev_dbg(pipe->asd->isp->dev, "WDT will hit after %d ms (%s)\n",
1737                         ((int)(next - jiffies) * 1000 / HZ), pipe->vdev.name);
1738 #endif
1739         else
1740 #ifndef ISP2401
1741                 dev_dbg(asd->isp->dev, "WDT starts with %d ms period\n",
1742                         ((int)(next - jiffies) * 1000 / HZ));
1743 #else
1744                 dev_dbg(pipe->asd->isp->dev, "WDT starts with %d ms period (%s)\n",
1745                         ((int)(next - jiffies) * 1000 / HZ), pipe->vdev.name);
1746 #endif
1747
1748 #ifndef ISP2401
1749         mod_timer(&asd->wdt, next);
1750         atomic_set(&asd->isp->wdt_count, 0);
1751 #else
1752         mod_timer(&pipe->wdt, next);
1753 #endif
1754 }
1755
1756 #ifndef ISP2401
1757 void atomisp_wdt_stop(struct atomisp_sub_device *asd, bool sync)
1758 #else
1759 void atomisp_wdt_refresh(struct atomisp_sub_device *asd, unsigned int delay)
1760 {
1761         dev_dbg(asd->isp->dev, "WDT refresh all:\n");
1762         if (atomisp_is_wdt_running(&asd->video_out_capture))
1763                 atomisp_wdt_refresh_pipe(&asd->video_out_capture, delay);
1764         if (atomisp_is_wdt_running(&asd->video_out_preview))
1765                 atomisp_wdt_refresh_pipe(&asd->video_out_preview, delay);
1766         if (atomisp_is_wdt_running(&asd->video_out_vf))
1767                 atomisp_wdt_refresh_pipe(&asd->video_out_vf, delay);
1768         if (atomisp_is_wdt_running(&asd->video_out_video_capture))
1769                 atomisp_wdt_refresh_pipe(&asd->video_out_video_capture, delay);
1770 }
1771
1772
1773 void atomisp_wdt_stop_pipe(struct atomisp_video_pipe *pipe, bool sync)
1774 #endif
1775 {
1776 #ifndef ISP2401
1777         dev_dbg(asd->isp->dev, "WDT stop\n");
1778 #else
1779         if (!atomisp_is_wdt_running(pipe))
1780                 return;
1781
1782         dev_dbg(pipe->asd->isp->dev,
1783                 "WDT stop asd %d (%s)\n", pipe->asd->index, pipe->vdev.name);
1784
1785 #endif
1786         if (sync) {
1787 #ifndef ISP2401
1788                 del_timer_sync(&asd->wdt);
1789                 cancel_work_sync(&asd->isp->wdt_work);
1790 #else
1791                 del_timer_sync(&pipe->wdt);
1792                 cancel_work_sync(&pipe->asd->isp->wdt_work);
1793 #endif
1794         } else {
1795 #ifndef ISP2401
1796                 del_timer(&asd->wdt);
1797 #else
1798                 del_timer(&pipe->wdt);
1799 #endif
1800         }
1801 }
1802
1803 #ifndef ISP2401
1804 void atomisp_wdt_start(struct atomisp_sub_device *asd)
1805 #else
1806 void atomisp_wdt_stop(struct atomisp_sub_device *asd, bool sync)
1807 {
1808         dev_dbg(asd->isp->dev, "WDT stop all:\n");
1809         atomisp_wdt_stop_pipe(&asd->video_out_capture, sync);
1810         atomisp_wdt_stop_pipe(&asd->video_out_preview, sync);
1811         atomisp_wdt_stop_pipe(&asd->video_out_vf, sync);
1812         atomisp_wdt_stop_pipe(&asd->video_out_video_capture, sync);
1813 }
1814
1815 void atomisp_wdt_start(struct atomisp_video_pipe *pipe)
1816 #endif
1817 {
1818 #ifndef ISP2401
1819         atomisp_wdt_refresh(asd, ATOMISP_ISP_TIMEOUT_DURATION);
1820 #else
1821         atomisp_wdt_refresh_pipe(pipe, ATOMISP_ISP_TIMEOUT_DURATION);
1822 #endif
1823 }
1824
1825 void atomisp_setup_flash(struct atomisp_sub_device *asd)
1826 {
1827         struct atomisp_device *isp = asd->isp;
1828         struct v4l2_control ctrl;
1829
1830         if (isp->flash == NULL)
1831                 return;
1832
1833         if (asd->params.flash_state != ATOMISP_FLASH_REQUESTED &&
1834             asd->params.flash_state != ATOMISP_FLASH_DONE)
1835                 return;
1836
1837         if (asd->params.num_flash_frames) {
1838                 /* make sure the timeout is set before setting flash mode */
1839                 ctrl.id = V4L2_CID_FLASH_TIMEOUT;
1840                 ctrl.value = FLASH_TIMEOUT;
1841
1842                 if (v4l2_s_ctrl(NULL, isp->flash->ctrl_handler, &ctrl)) {
1843                         dev_err(isp->dev, "flash timeout configure failed\n");
1844                         return;
1845                 }
1846
1847                 atomisp_css_request_flash(asd);
1848                 asd->params.flash_state = ATOMISP_FLASH_ONGOING;
1849         } else {
1850                 asd->params.flash_state = ATOMISP_FLASH_IDLE;
1851         }
1852 }
1853
1854 irqreturn_t atomisp_isr_thread(int irq, void *isp_ptr)
1855 {
1856         struct atomisp_device *isp = isp_ptr;
1857         unsigned long flags;
1858         bool frame_done_found[MAX_STREAM_NUM] = {0};
1859         bool css_pipe_done[MAX_STREAM_NUM] = {0};
1860         unsigned int i;
1861         struct atomisp_sub_device *asd = &isp->asd[0];
1862
1863         dev_dbg(isp->dev, ">%s\n", __func__);
1864
1865         spin_lock_irqsave(&isp->lock, flags);
1866
1867         if (!atomisp_streaming_count(isp) && !atomisp_is_acc_enabled(isp)) {
1868                 spin_unlock_irqrestore(&isp->lock, flags);
1869                 return IRQ_HANDLED;
1870         }
1871
1872         spin_unlock_irqrestore(&isp->lock, flags);
1873
1874         /*
1875          * The standard CSS2.0 API tells the following calling sequence of
1876          * dequeue ready buffers:
1877          * while (ia_css_dequeue_event(...)) {
1878          *      switch (event.type) {
1879          *      ...
1880          *      ia_css_pipe_dequeue_buffer()
1881          *      }
1882          * }
1883          * That is, dequeue event and buffer are one after another.
1884          *
1885          * But the following implementation is to first deuque all the event
1886          * to a FIFO, then process the event in the FIFO.
1887          * This will not have issue in single stream mode, but it do have some
1888          * issue in multiple stream case. The issue is that
1889          * ia_css_pipe_dequeue_buffer() will not return the corrent buffer in
1890          * a specific pipe.
1891          *
1892          * This is due to ia_css_pipe_dequeue_buffer() does not take the
1893          * ia_css_pipe parameter.
1894          *
1895          * So:
1896          * For CSS2.0: we change the way to not dequeue all the event at one
1897          * time, instead, dequue one and process one, then another
1898          */
1899         rt_mutex_lock(&isp->mutex);
1900         if (atomisp_css_isr_thread(isp, frame_done_found, css_pipe_done))
1901                 goto out;
1902
1903         for (i = 0; i < isp->num_of_streams; i++) {
1904                 asd = &isp->asd[i];
1905                 if (asd->streaming != ATOMISP_DEVICE_STREAMING_ENABLED)
1906                         continue;
1907                 atomisp_setup_flash(asd);
1908
1909         }
1910 out:
1911         rt_mutex_unlock(&isp->mutex);
1912         for (i = 0; i < isp->num_of_streams; i++) {
1913                 asd = &isp->asd[i];
1914                 if (asd->streaming == ATOMISP_DEVICE_STREAMING_ENABLED
1915                     && css_pipe_done[asd->index]
1916                     && isp->sw_contex.file_input)
1917                         v4l2_subdev_call(isp->inputs[asd->input_curr].camera,
1918                                          video, s_stream, 1);
1919                 /* FIXME! FIX ACC implementation */
1920                 if (asd->acc.pipeline && css_pipe_done[asd->index])
1921                         atomisp_css_acc_done(asd);
1922         }
1923         dev_dbg(isp->dev, "<%s\n", __func__);
1924
1925         return IRQ_HANDLED;
1926 }
1927
1928 /*
1929  * utils for buffer allocation/free
1930  */
1931
1932 int atomisp_get_frame_pgnr(struct atomisp_device *isp,
1933                            const struct atomisp_css_frame *frame, u32 *p_pgnr)
1934 {
1935         if (!frame) {
1936                 dev_err(isp->dev, "%s: NULL frame pointer ERROR.\n", __func__);
1937                 return -EINVAL;
1938         }
1939
1940         *p_pgnr = DIV_ROUND_UP(frame->data_bytes, PAGE_SIZE);
1941         return 0;
1942 }
1943
1944 /*
1945  * Get internal fmt according to V4L2 fmt
1946  */
1947 static enum atomisp_css_frame_format
1948 v4l2_fmt_to_sh_fmt(u32 fmt)
1949 {
1950         switch (fmt) {
1951         case V4L2_PIX_FMT_YUV420:
1952                 return CSS_FRAME_FORMAT_YUV420;
1953         case V4L2_PIX_FMT_YVU420:
1954                 return CSS_FRAME_FORMAT_YV12;
1955         case V4L2_PIX_FMT_YUV422P:
1956                 return CSS_FRAME_FORMAT_YUV422;
1957         case V4L2_PIX_FMT_YUV444:
1958                 return CSS_FRAME_FORMAT_YUV444;
1959         case V4L2_PIX_FMT_NV12:
1960                 return CSS_FRAME_FORMAT_NV12;
1961         case V4L2_PIX_FMT_NV21:
1962                 return CSS_FRAME_FORMAT_NV21;
1963         case V4L2_PIX_FMT_NV16:
1964                 return CSS_FRAME_FORMAT_NV16;
1965         case V4L2_PIX_FMT_NV61:
1966                 return CSS_FRAME_FORMAT_NV61;
1967         case V4L2_PIX_FMT_UYVY:
1968                 return CSS_FRAME_FORMAT_UYVY;
1969         case V4L2_PIX_FMT_YUYV:
1970                 return CSS_FRAME_FORMAT_YUYV;
1971         case V4L2_PIX_FMT_RGB24:
1972                 return CSS_FRAME_FORMAT_PLANAR_RGB888;
1973         case V4L2_PIX_FMT_RGB32:
1974                 return CSS_FRAME_FORMAT_RGBA888;
1975         case V4L2_PIX_FMT_RGB565:
1976                 return CSS_FRAME_FORMAT_RGB565;
1977         case V4L2_PIX_FMT_JPEG:
1978         case V4L2_PIX_FMT_CUSTOM_M10MO_RAW:
1979                 return CSS_FRAME_FORMAT_BINARY_8;
1980         case V4L2_PIX_FMT_SBGGR16:
1981         case V4L2_PIX_FMT_SBGGR10:
1982         case V4L2_PIX_FMT_SGBRG10:
1983         case V4L2_PIX_FMT_SGRBG10:
1984         case V4L2_PIX_FMT_SRGGB10:
1985         case V4L2_PIX_FMT_SBGGR12:
1986         case V4L2_PIX_FMT_SGBRG12:
1987         case V4L2_PIX_FMT_SGRBG12:
1988         case V4L2_PIX_FMT_SRGGB12:
1989         case V4L2_PIX_FMT_SBGGR8:
1990         case V4L2_PIX_FMT_SGBRG8:
1991         case V4L2_PIX_FMT_SGRBG8:
1992         case V4L2_PIX_FMT_SRGGB8:
1993                 return CSS_FRAME_FORMAT_RAW;
1994         default:
1995                 return -EINVAL;
1996         }
1997 }
1998 /*
1999  * raw format match between SH format and V4L2 format
2000  */
2001 static int raw_output_format_match_input(u32 input, u32 output)
2002 {
2003         if ((input == CSS_FORMAT_RAW_12) &&
2004             ((output == V4L2_PIX_FMT_SRGGB12) ||
2005              (output == V4L2_PIX_FMT_SGRBG12) ||
2006              (output == V4L2_PIX_FMT_SBGGR12) ||
2007              (output == V4L2_PIX_FMT_SGBRG12)))
2008                 return 0;
2009
2010         if ((input == CSS_FORMAT_RAW_10) &&
2011             ((output == V4L2_PIX_FMT_SRGGB10) ||
2012              (output == V4L2_PIX_FMT_SGRBG10) ||
2013              (output == V4L2_PIX_FMT_SBGGR10) ||
2014              (output == V4L2_PIX_FMT_SGBRG10)))
2015                 return 0;
2016
2017         if ((input == CSS_FORMAT_RAW_8) &&
2018             ((output == V4L2_PIX_FMT_SRGGB8) ||
2019              (output == V4L2_PIX_FMT_SGRBG8) ||
2020              (output == V4L2_PIX_FMT_SBGGR8) ||
2021              (output == V4L2_PIX_FMT_SGBRG8)))
2022                 return 0;
2023
2024         if ((input == CSS_FORMAT_RAW_16) && (output == V4L2_PIX_FMT_SBGGR16))
2025                 return 0;
2026
2027         return -EINVAL;
2028 }
2029
2030 static u32 get_pixel_depth(u32 pixelformat)
2031 {
2032         switch (pixelformat) {
2033         case V4L2_PIX_FMT_YUV420:
2034         case V4L2_PIX_FMT_NV12:
2035         case V4L2_PIX_FMT_NV21:
2036         case V4L2_PIX_FMT_YVU420:
2037                 return 12;
2038         case V4L2_PIX_FMT_YUV422P:
2039         case V4L2_PIX_FMT_YUYV:
2040         case V4L2_PIX_FMT_UYVY:
2041         case V4L2_PIX_FMT_NV16:
2042         case V4L2_PIX_FMT_NV61:
2043         case V4L2_PIX_FMT_RGB565:
2044         case V4L2_PIX_FMT_SBGGR16:
2045         case V4L2_PIX_FMT_SBGGR12:
2046         case V4L2_PIX_FMT_SGBRG12:
2047         case V4L2_PIX_FMT_SGRBG12:
2048         case V4L2_PIX_FMT_SRGGB12:
2049         case V4L2_PIX_FMT_SBGGR10:
2050         case V4L2_PIX_FMT_SGBRG10:
2051         case V4L2_PIX_FMT_SGRBG10:
2052         case V4L2_PIX_FMT_SRGGB10:
2053                 return 16;
2054         case V4L2_PIX_FMT_RGB24:
2055         case V4L2_PIX_FMT_YUV444:
2056                 return 24;
2057         case V4L2_PIX_FMT_RGB32:
2058                 return 32;
2059         case V4L2_PIX_FMT_JPEG:
2060         case V4L2_PIX_FMT_CUSTOM_M10MO_RAW:
2061         case V4L2_PIX_FMT_SBGGR8:
2062         case V4L2_PIX_FMT_SGBRG8:
2063         case V4L2_PIX_FMT_SGRBG8:
2064         case V4L2_PIX_FMT_SRGGB8:
2065                 return 8;
2066         default:
2067                 return 8 * 2;   /* raw type now */
2068         }
2069 }
2070
2071 bool atomisp_is_mbuscode_raw(uint32_t code)
2072 {
2073         return code >= 0x3000 && code < 0x4000;
2074 }
2075
2076 /*
2077  * ISP features control function
2078  */
2079
2080 /*
2081  * Set ISP capture mode based on current settings
2082  */
2083 static void atomisp_update_capture_mode(struct atomisp_sub_device *asd)
2084 {
2085         if (asd->params.gdc_cac_en)
2086                 atomisp_css_capture_set_mode(asd, CSS_CAPTURE_MODE_ADVANCED);
2087         else if (asd->params.low_light)
2088                 atomisp_css_capture_set_mode(asd, CSS_CAPTURE_MODE_LOW_LIGHT);
2089         else if (asd->video_out_capture.sh_fmt == CSS_FRAME_FORMAT_RAW)
2090                 atomisp_css_capture_set_mode(asd, CSS_CAPTURE_MODE_RAW);
2091         else
2092                 atomisp_css_capture_set_mode(asd, CSS_CAPTURE_MODE_PRIMARY);
2093 }
2094
2095 #ifdef ISP2401
2096 int atomisp_set_sensor_runmode(struct atomisp_sub_device *asd,
2097                 struct atomisp_s_runmode *runmode)
2098 {
2099         struct atomisp_device *isp = asd->isp;
2100         struct v4l2_ctrl *c;
2101         struct v4l2_streamparm p = {0};
2102         int ret;
2103         int modes[] = { CI_MODE_NONE,
2104                         CI_MODE_VIDEO,
2105                         CI_MODE_STILL_CAPTURE,
2106                         CI_MODE_CONTINUOUS,
2107                         CI_MODE_PREVIEW };
2108
2109         if (!(runmode && (runmode->mode & RUNMODE_MASK)))
2110                 return -EINVAL;
2111
2112         mutex_lock(asd->ctrl_handler.lock);
2113         c = v4l2_ctrl_find(isp->inputs[asd->input_curr].camera->ctrl_handler,
2114                            V4L2_CID_RUN_MODE);
2115
2116         if (c) {
2117                 ret = v4l2_ctrl_s_ctrl(c, runmode->mode);
2118         } else {
2119                 p.parm.capture.capturemode = modes[runmode->mode];
2120                 ret = v4l2_subdev_call(isp->inputs[asd->input_curr].camera,
2121                                        video, s_parm, &p);
2122         }
2123
2124         mutex_unlock(asd->ctrl_handler.lock);
2125         return ret;
2126 }
2127
2128 #endif
2129 /*
2130  * Function to enable/disable lens geometry distortion correction (GDC) and
2131  * chromatic aberration correction (CAC)
2132  */
2133 int atomisp_gdc_cac(struct atomisp_sub_device *asd, int flag,
2134                     __s32 *value)
2135 {
2136         if (flag == 0) {
2137                 *value = asd->params.gdc_cac_en;
2138                 return 0;
2139         }
2140
2141         asd->params.gdc_cac_en = !!*value;
2142         if (asd->params.gdc_cac_en) {
2143                 atomisp_css_set_morph_table(asd,
2144                                             asd->params.css_param.morph_table);
2145         } else {
2146                 atomisp_css_set_morph_table(asd, NULL);
2147         }
2148         asd->params.css_update_params_needed = true;
2149         atomisp_update_capture_mode(asd);
2150         return 0;
2151 }
2152
2153 /*
2154  * Function to enable/disable low light mode including ANR
2155  */
2156 int atomisp_low_light(struct atomisp_sub_device *asd, int flag,
2157                       __s32 *value)
2158 {
2159         if (flag == 0) {
2160                 *value = asd->params.low_light;
2161                 return 0;
2162         }
2163
2164         asd->params.low_light = (*value != 0);
2165         atomisp_update_capture_mode(asd);
2166         return 0;
2167 }
2168
2169 /*
2170  * Function to enable/disable extra noise reduction (XNR) in low light
2171  * condition
2172  */
2173 int atomisp_xnr(struct atomisp_sub_device *asd, int flag,
2174                 int *xnr_enable)
2175 {
2176         if (flag == 0) {
2177                 *xnr_enable = asd->params.xnr_en;
2178                 return 0;
2179         }
2180
2181         atomisp_css_capture_enable_xnr(asd, !!*xnr_enable);
2182
2183         return 0;
2184 }
2185
2186 /*
2187  * Function to configure bayer noise reduction
2188  */
2189 int atomisp_nr(struct atomisp_sub_device *asd, int flag,
2190                struct atomisp_nr_config *arg)
2191 {
2192         if (flag == 0) {
2193                 /* Get nr config from current setup */
2194                 if (atomisp_css_get_nr_config(asd, arg))
2195                         return -EINVAL;
2196         } else {
2197                 /* Set nr config to isp parameters */
2198                 memcpy(&asd->params.css_param.nr_config, arg,
2199                        sizeof(struct atomisp_css_nr_config));
2200                 atomisp_css_set_nr_config(asd, &asd->params.css_param.nr_config);
2201                 asd->params.css_update_params_needed = true;
2202         }
2203         return 0;
2204 }
2205
2206 /*
2207  * Function to configure temporal noise reduction (TNR)
2208  */
2209 int atomisp_tnr(struct atomisp_sub_device *asd, int flag,
2210                 struct atomisp_tnr_config *config)
2211 {
2212         /* Get tnr config from current setup */
2213         if (flag == 0) {
2214                 /* Get tnr config from current setup */
2215                 if (atomisp_css_get_tnr_config(asd, config))
2216                         return -EINVAL;
2217         } else {
2218                 /* Set tnr config to isp parameters */
2219                 memcpy(&asd->params.css_param.tnr_config, config,
2220                         sizeof(struct atomisp_css_tnr_config));
2221                 atomisp_css_set_tnr_config(asd, &asd->params.css_param.tnr_config);
2222                 asd->params.css_update_params_needed = true;
2223         }
2224
2225         return 0;
2226 }
2227
2228 /*
2229  * Function to configure black level compensation
2230  */
2231 int atomisp_black_level(struct atomisp_sub_device *asd, int flag,
2232                         struct atomisp_ob_config *config)
2233 {
2234         if (flag == 0) {
2235                 /* Get ob config from current setup */
2236                 if (atomisp_css_get_ob_config(asd, config))
2237                         return -EINVAL;
2238         } else {
2239                 /* Set ob config to isp parameters */
2240                 memcpy(&asd->params.css_param.ob_config, config,
2241                        sizeof(struct atomisp_css_ob_config));
2242                 atomisp_css_set_ob_config(asd, &asd->params.css_param.ob_config);
2243                 asd->params.css_update_params_needed = true;
2244         }
2245
2246         return 0;
2247 }
2248
2249 /*
2250  * Function to configure edge enhancement
2251  */
2252 int atomisp_ee(struct atomisp_sub_device *asd, int flag,
2253                struct atomisp_ee_config *config)
2254 {
2255         if (flag == 0) {
2256                 /* Get ee config from current setup */
2257                 if (atomisp_css_get_ee_config(asd, config))
2258                         return -EINVAL;
2259         } else {
2260                 /* Set ee config to isp parameters */
2261                 memcpy(&asd->params.css_param.ee_config, config,
2262                        sizeof(asd->params.css_param.ee_config));
2263                 atomisp_css_set_ee_config(asd, &asd->params.css_param.ee_config);
2264                 asd->params.css_update_params_needed = true;
2265         }
2266
2267         return 0;
2268 }
2269
2270 /*
2271  * Function to update Gamma table for gamma, brightness and contrast config
2272  */
2273 int atomisp_gamma(struct atomisp_sub_device *asd, int flag,
2274                   struct atomisp_gamma_table *config)
2275 {
2276         if (flag == 0) {
2277                 /* Get gamma table from current setup */
2278                 if (atomisp_css_get_gamma_table(asd, config))
2279                         return -EINVAL;
2280         } else {
2281                 /* Set gamma table to isp parameters */
2282                 memcpy(&asd->params.css_param.gamma_table, config,
2283                        sizeof(asd->params.css_param.gamma_table));
2284                 atomisp_css_set_gamma_table(asd, &asd->params.css_param.gamma_table);
2285         }
2286
2287         return 0;
2288 }
2289
2290 /*
2291  * Function to update Ctc table for Chroma Enhancement
2292  */
2293 int atomisp_ctc(struct atomisp_sub_device *asd, int flag,
2294                 struct atomisp_ctc_table *config)
2295 {
2296         if (flag == 0) {
2297                 /* Get ctc table from current setup */
2298                 if (atomisp_css_get_ctc_table(asd, config))
2299                         return -EINVAL;
2300         } else {
2301                 /* Set ctc table to isp parameters */
2302                 memcpy(&asd->params.css_param.ctc_table, config,
2303                        sizeof(asd->params.css_param.ctc_table));
2304                 atomisp_css_set_ctc_table(asd, &asd->params.css_param.ctc_table);
2305         }
2306
2307         return 0;
2308 }
2309
2310 /*
2311  * Function to update gamma correction parameters
2312  */
2313 int atomisp_gamma_correction(struct atomisp_sub_device *asd, int flag,
2314         struct atomisp_gc_config *config)
2315 {
2316         if (flag == 0) {
2317                 /* Get gamma correction params from current setup */
2318                 if (atomisp_css_get_gc_config(asd, config))
2319                         return -EINVAL;
2320         } else {
2321                 /* Set gamma correction params to isp parameters */
2322                 memcpy(&asd->params.css_param.gc_config, config,
2323                        sizeof(asd->params.css_param.gc_config));
2324                 atomisp_css_set_gc_config(asd, &asd->params.css_param.gc_config);
2325                 asd->params.css_update_params_needed = true;
2326         }
2327
2328         return 0;
2329 }
2330
2331 /*
2332  * Function to update narrow gamma flag
2333  */
2334 int atomisp_formats(struct atomisp_sub_device *asd, int flag,
2335                     struct atomisp_formats_config *config)
2336 {
2337         if (flag == 0) {
2338                 /* Get narrow gamma flag from current setup */
2339                 if (atomisp_css_get_formats_config(asd, config))
2340                         return -EINVAL;
2341         } else {
2342                 /* Set narrow gamma flag to isp parameters */
2343                 memcpy(&asd->params.css_param.formats_config, config,
2344                        sizeof(asd->params.css_param.formats_config));
2345                 atomisp_css_set_formats_config(asd, &asd->params.css_param.formats_config);
2346         }
2347
2348         return 0;
2349 }
2350
2351 void atomisp_free_internal_buffers(struct atomisp_sub_device *asd)
2352 {
2353         atomisp_free_css_parameters(&asd->params.css_param);
2354
2355         if (asd->raw_output_frame) {
2356                 atomisp_css_frame_free(asd->raw_output_frame);
2357                 asd->raw_output_frame = NULL;
2358         }
2359 }
2360
2361 static void atomisp_update_grid_info(struct atomisp_sub_device *asd,
2362                                      enum atomisp_css_pipe_id pipe_id,
2363                                      int source_pad)
2364 {
2365         struct atomisp_device *isp = asd->isp;
2366         int err;
2367         uint16_t stream_id = atomisp_source_pad_to_stream_id(asd, source_pad);
2368
2369         if (atomisp_css_get_grid_info(asd, pipe_id, source_pad))
2370                 return;
2371
2372         /* We must free all buffers because they no longer match
2373            the grid size. */
2374         atomisp_css_free_stat_buffers(asd);
2375
2376         err = atomisp_alloc_css_stat_bufs(asd, stream_id);
2377         if (err) {
2378                 dev_err(isp->dev, "stat_buf allocate error\n");
2379                 goto err;
2380         }
2381
2382         if (atomisp_alloc_3a_output_buf(asd)) {
2383                 /* Failure for 3A buffers does not influence DIS buffers */
2384                 if (asd->params.s3a_output_bytes != 0) {
2385                         /* For SOC sensor happens s3a_output_bytes == 0,
2386                          * using if condition to exclude false error log */
2387                         dev_err(isp->dev, "Failed to allocate memory for 3A statistics\n");
2388                 }
2389                 goto err;
2390         }
2391
2392         if (atomisp_alloc_dis_coef_buf(asd)) {
2393                 dev_err(isp->dev,
2394                         "Failed to allocate memory for DIS statistics\n");
2395                 goto err;
2396         }
2397
2398         if (atomisp_alloc_metadata_output_buf(asd)) {
2399                 dev_err(isp->dev, "Failed to allocate memory for metadata\n");
2400                 goto err;
2401         }
2402
2403         return;
2404
2405 err:
2406         atomisp_css_free_stat_buffers(asd);
2407         return;
2408 }
2409
2410 static void atomisp_curr_user_grid_info(struct atomisp_sub_device *asd,
2411                                         struct atomisp_grid_info *info)
2412 {
2413         memcpy(info, &asd->params.curr_grid_info.s3a_grid,
2414                sizeof(struct atomisp_css_3a_grid_info));
2415 }
2416
2417 int atomisp_compare_grid(struct atomisp_sub_device *asd,
2418                                 struct atomisp_grid_info *atomgrid)
2419 {
2420         struct atomisp_grid_info tmp = {0};
2421
2422         atomisp_curr_user_grid_info(asd, &tmp);
2423         return memcmp(atomgrid, &tmp, sizeof(tmp));
2424 }
2425
2426 /*
2427  * Function to update Gdc table for gdc
2428  */
2429 int atomisp_gdc_cac_table(struct atomisp_sub_device *asd, int flag,
2430                           struct atomisp_morph_table *config)
2431 {
2432         int ret;
2433         int i;
2434         struct atomisp_device *isp = asd->isp;
2435
2436         if (flag == 0) {
2437                 /* Get gdc table from current setup */
2438                 struct atomisp_css_morph_table tab = {0};
2439                 atomisp_css_get_morph_table(asd, &tab);
2440
2441                 config->width = tab.width;
2442                 config->height = tab.height;
2443
2444                 for (i = 0; i < CSS_MORPH_TABLE_NUM_PLANES; i++) {
2445                         ret = copy_to_user(config->coordinates_x[i],
2446                                 tab.coordinates_x[i], tab.height *
2447                                 tab.width * sizeof(*tab.coordinates_x[i]));
2448                         if (ret) {
2449                                 dev_err(isp->dev,
2450                                         "Failed to copy to User for x\n");
2451                                 return -EFAULT;
2452                         }
2453                         ret = copy_to_user(config->coordinates_y[i],
2454                                 tab.coordinates_y[i], tab.height *
2455                                 tab.width * sizeof(*tab.coordinates_y[i]));
2456                         if (ret) {
2457                                 dev_err(isp->dev,
2458                                         "Failed to copy to User for y\n");
2459                                 return -EFAULT;
2460                         }
2461                 }
2462         } else {
2463                 struct atomisp_css_morph_table *tab =
2464                         asd->params.css_param.morph_table;
2465
2466                 /* free first if we have one */
2467                 if (tab) {
2468                         atomisp_css_morph_table_free(tab);
2469                         asd->params.css_param.morph_table = NULL;
2470                 }
2471
2472                 /* allocate new one */
2473                 tab = atomisp_css_morph_table_allocate(config->width,
2474                                                        config->height);
2475
2476                 if (!tab) {
2477                         dev_err(isp->dev, "out of memory\n");
2478                         return -EINVAL;
2479                 }
2480
2481                 for (i = 0; i < CSS_MORPH_TABLE_NUM_PLANES; i++) {
2482                         ret = copy_from_user(tab->coordinates_x[i],
2483                                 config->coordinates_x[i],
2484                                 config->height * config->width *
2485                                 sizeof(*config->coordinates_x[i]));
2486                         if (ret) {
2487                                 dev_err(isp->dev,
2488                                 "Failed to copy from User for x, ret %d\n",
2489                                 ret);
2490                                 atomisp_css_morph_table_free(tab);
2491                                 return -EFAULT;
2492                         }
2493                         ret = copy_from_user(tab->coordinates_y[i],
2494                                 config->coordinates_y[i],
2495                                 config->height * config->width *
2496                                 sizeof(*config->coordinates_y[i]));
2497                         if (ret) {
2498                                 dev_err(isp->dev,
2499                                 "Failed to copy from User for y, ret is %d\n",
2500                                 ret);
2501                                 atomisp_css_morph_table_free(tab);
2502                                 return -EFAULT;
2503                         }
2504                 }
2505                 asd->params.css_param.morph_table = tab;
2506                 if (asd->params.gdc_cac_en)
2507                         atomisp_css_set_morph_table(asd, tab);
2508         }
2509
2510         return 0;
2511 }
2512
2513 int atomisp_macc_table(struct atomisp_sub_device *asd, int flag,
2514                        struct atomisp_macc_config *config)
2515 {
2516         struct atomisp_css_macc_table *macc_table;
2517
2518         switch (config->color_effect) {
2519         case V4L2_COLORFX_NONE:
2520                 macc_table = &asd->params.css_param.macc_table;
2521                 break;
2522         case V4L2_COLORFX_SKY_BLUE:
2523                 macc_table = &blue_macc_table;
2524                 break;
2525         case V4L2_COLORFX_GRASS_GREEN:
2526                 macc_table = &green_macc_table;
2527                 break;
2528         case V4L2_COLORFX_SKIN_WHITEN_LOW:
2529                 macc_table = &skin_low_macc_table;
2530                 break;
2531         case V4L2_COLORFX_SKIN_WHITEN:
2532                 macc_table = &skin_medium_macc_table;
2533                 break;
2534         case V4L2_COLORFX_SKIN_WHITEN_HIGH:
2535                 macc_table = &skin_high_macc_table;
2536                 break;
2537         default:
2538                 return -EINVAL;
2539         }
2540
2541         if (flag == 0) {
2542                 /* Get macc table from current setup */
2543                 memcpy(&config->table, macc_table,
2544                        sizeof(struct atomisp_css_macc_table));
2545         } else {
2546                 memcpy(macc_table, &config->table,
2547                        sizeof(struct atomisp_css_macc_table));
2548                 if (config->color_effect == asd->params.color_effect)
2549                         atomisp_css_set_macc_table(asd, macc_table);
2550         }
2551
2552         return 0;
2553 }
2554
2555 int atomisp_set_dis_vector(struct atomisp_sub_device *asd,
2556                            struct atomisp_dis_vector *vector)
2557 {
2558         atomisp_css_video_set_dis_vector(asd, vector);
2559
2560         asd->params.dis_proj_data_valid = false;
2561         asd->params.css_update_params_needed = true;
2562         return 0;
2563 }
2564
2565 /*
2566  * Function to set/get image stablization statistics
2567  */
2568 int atomisp_get_dis_stat(struct atomisp_sub_device *asd,
2569                          struct atomisp_dis_statistics *stats)
2570 {
2571         return atomisp_css_get_dis_stat(asd, stats);
2572 }
2573
2574 /*
2575  * Function  set camrea_prefiles.xml current sensor pixel array size
2576  */
2577 int atomisp_set_array_res(struct atomisp_sub_device *asd,
2578                          struct atomisp_resolution  *config)
2579 {
2580         dev_dbg(asd->isp->dev, ">%s start\n", __func__);
2581         if (!config) {
2582                 dev_err(asd->isp->dev, "Set sensor array size is not valid\n");
2583                 return -EINVAL;
2584         }
2585
2586         asd->sensor_array_res.width = config->width;
2587         asd->sensor_array_res.height = config->height;
2588         return 0;
2589 }
2590
2591 /*
2592  * Function to get DVS2 BQ resolution settings
2593  */
2594 int atomisp_get_dvs2_bq_resolutions(struct atomisp_sub_device *asd,
2595                          struct atomisp_dvs2_bq_resolutions *bq_res)
2596 {
2597         struct ia_css_pipe_config *pipe_cfg = NULL;
2598         struct ia_css_stream_config *stream_cfg = NULL;
2599         struct ia_css_stream_input_config *input_config = NULL;
2600
2601         struct ia_css_stream *stream =
2602                 asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].stream;
2603         if (!stream) {
2604                 dev_warn(asd->isp->dev, "stream is not created");
2605                 return -EAGAIN;
2606         }
2607
2608         pipe_cfg = &asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL]
2609                 .pipe_configs[CSS_PIPE_ID_VIDEO];
2610         stream_cfg = &asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL]
2611                 .stream_config;
2612         input_config = &stream_cfg->input_config;
2613
2614         if (!bq_res)
2615                 return -EINVAL;
2616
2617         /* the GDC output resolution */
2618         bq_res->output_bq.width_bq = pipe_cfg->output_info[0].res.width / 2;
2619         bq_res->output_bq.height_bq = pipe_cfg->output_info[0].res.height / 2;
2620
2621         bq_res->envelope_bq.width_bq = 0;
2622         bq_res->envelope_bq.height_bq = 0;
2623         /* the GDC input resolution */
2624         if (!asd->continuous_mode->val) {
2625                 bq_res->source_bq.width_bq = bq_res->output_bq.width_bq +
2626                                 pipe_cfg->dvs_envelope.width / 2;
2627                 bq_res->source_bq.height_bq = bq_res->output_bq.height_bq +
2628                                 pipe_cfg->dvs_envelope.height / 2;
2629                 /*
2630                  * Bad pixels caused by spatial filter processing
2631                  * ISP filter resolution should be given by CSS/FW, but for now
2632                  * there is not such API to query, and it is fixed value, so
2633                  * hardcoded here.
2634                  */
2635                 bq_res->ispfilter_bq.width_bq = 12 / 2;
2636                 bq_res->ispfilter_bq.height_bq = 12 / 2;
2637                 /* spatial filter shift, always 4 pixels */
2638                 bq_res->gdc_shift_bq.width_bq = 4 / 2;
2639                 bq_res->gdc_shift_bq.height_bq = 4 / 2;
2640
2641                 if (asd->params.video_dis_en) {
2642                         bq_res->envelope_bq.width_bq = pipe_cfg->dvs_envelope.width
2643                                         / 2 - bq_res->ispfilter_bq.width_bq;
2644                         bq_res->envelope_bq.height_bq = pipe_cfg->dvs_envelope.height
2645                                         / 2 - bq_res->ispfilter_bq.height_bq;
2646                 }
2647         } else {
2648                 unsigned int w_padding;
2649                 unsigned int gdc_effective_input = 0;
2650
2651                 /* For GDC:
2652                  * gdc_effective_input = effective_input + envelope
2653                  *
2654                  * From the comment and formula in BZ1786,
2655                  * we see the source_bq should be:
2656                  * effective_input / bayer_ds_ratio
2657                  */
2658                 bq_res->source_bq.width_bq =
2659                         (input_config->effective_res.width *
2660                          pipe_cfg->bayer_ds_out_res.width /
2661                          input_config->effective_res.width + 1) / 2;
2662                 bq_res->source_bq.height_bq =
2663                         (input_config->effective_res.height *
2664                          pipe_cfg->bayer_ds_out_res.height /
2665                          input_config->effective_res.height + 1) / 2;
2666
2667
2668                 if (!asd->params.video_dis_en) {
2669                         /*
2670                          * We adjust the ispfilter_bq to:
2671                          * ispfilter_bq = 128/BDS
2672                          * we still need firmware team to provide an offical
2673                          * formula for SDV.
2674                          */
2675                         bq_res->ispfilter_bq.width_bq = 128 *
2676                                 pipe_cfg->bayer_ds_out_res.width /
2677                                 input_config->effective_res.width / 2;
2678                         bq_res->ispfilter_bq.height_bq = 128 *
2679                                 pipe_cfg->bayer_ds_out_res.width /
2680                                 input_config->effective_res.width / 2;
2681
2682                         if (IS_HWREVISION(asd->isp, ATOMISP_HW_REVISION_ISP2401)) {
2683                                 /* No additional left padding for ISYS2401 */
2684                                 bq_res->gdc_shift_bq.width_bq = 4 / 2;
2685                                 bq_res->gdc_shift_bq.height_bq = 4 / 2;
2686                         } else {
2687                                 /*
2688                                  * For the w_padding and gdc_shift_bq cacluation
2689                                  * Please see the BZ 1786 and 4358 for more info.
2690                                  * Just test that this formula can work now,
2691                                  * but we still have no offical formula.
2692                                  *
2693                                  * w_padding = ceiling(gdc_effective_input
2694                                  *             /128, 1) * 128 - effective_width
2695                                  * gdc_shift_bq = w_padding/BDS/2 + ispfilter_bq/2
2696                                  */
2697                                 gdc_effective_input =
2698                                         input_config->effective_res.width +
2699                                         pipe_cfg->dvs_envelope.width;
2700                                 w_padding = roundup(gdc_effective_input, 128) -
2701                                         input_config->effective_res.width;
2702                                 w_padding = w_padding *
2703                                         pipe_cfg->bayer_ds_out_res.width /
2704                                         input_config->effective_res.width + 1;
2705                                 w_padding = roundup(w_padding/2, 1);
2706
2707                                 bq_res->gdc_shift_bq.width_bq = bq_res->ispfilter_bq.width_bq / 2
2708                                         + w_padding;
2709                                 bq_res->gdc_shift_bq.height_bq = 4 / 2;
2710                         }
2711                 } else {
2712                         unsigned int dvs_w, dvs_h, dvs_w_max, dvs_h_max;
2713
2714                         bq_res->ispfilter_bq.width_bq = 8 / 2;
2715                         bq_res->ispfilter_bq.height_bq = 8 / 2;
2716
2717                         if (IS_HWREVISION(asd->isp, ATOMISP_HW_REVISION_ISP2401)) {
2718                                 /* No additional left padding for ISYS2401 */
2719                                 bq_res->gdc_shift_bq.width_bq = 4 / 2;
2720                                 bq_res->gdc_shift_bq.height_bq = 4 / 2;
2721                         } else {
2722                                 w_padding =
2723                                     roundup(input_config->effective_res.width, 128) -
2724                                     input_config->effective_res.width;
2725                                 if (w_padding < 12)
2726                                         w_padding = 12;
2727                                 bq_res->gdc_shift_bq.width_bq = 4 / 2 +
2728                                     ((w_padding - 12) *
2729                                      pipe_cfg->bayer_ds_out_res.width /
2730                                 input_config->effective_res.width + 1) / 2;
2731                                 bq_res->gdc_shift_bq.height_bq = 4 / 2;
2732                         }
2733
2734                         dvs_w = pipe_cfg->bayer_ds_out_res.width -
2735                                 pipe_cfg->output_info[0].res.width;
2736                         dvs_h = pipe_cfg->bayer_ds_out_res.height -
2737                                 pipe_cfg->output_info[0].res.height;
2738                         dvs_w_max = rounddown(
2739                                         pipe_cfg->output_info[0].res.width / 5,
2740                                         ATOM_ISP_STEP_WIDTH);
2741                         dvs_h_max = rounddown(
2742                                         pipe_cfg->output_info[0].res.height / 5,
2743                                         ATOM_ISP_STEP_HEIGHT);
2744                         bq_res->envelope_bq.width_bq =
2745                                 min((dvs_w / 2), (dvs_w_max / 2)) -
2746                                 bq_res->ispfilter_bq.width_bq;
2747                         bq_res->envelope_bq.height_bq =
2748                                 min((dvs_h / 2), (dvs_h_max / 2)) -
2749                                 bq_res->ispfilter_bq.height_bq;
2750                 }
2751         }
2752
2753         dev_dbg(asd->isp->dev, "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",
2754               bq_res->source_bq.width_bq, bq_res->source_bq.height_bq,
2755               bq_res->ispfilter_bq.width_bq, bq_res->ispfilter_bq.height_bq,
2756               bq_res->gdc_shift_bq.width_bq, bq_res->gdc_shift_bq.height_bq,
2757               bq_res->envelope_bq.width_bq, bq_res->envelope_bq.height_bq,
2758               bq_res->output_bq.width_bq, bq_res->output_bq.height_bq);
2759
2760         return 0;
2761 }
2762
2763 int atomisp_set_dis_coefs(struct atomisp_sub_device *asd,
2764                           struct atomisp_dis_coefficients *coefs)
2765 {
2766         return atomisp_css_set_dis_coefs(asd, coefs);
2767 }
2768
2769 /*
2770  * Function to set/get 3A stat from isp
2771  */
2772 int atomisp_3a_stat(struct atomisp_sub_device *asd, int flag,
2773                     struct atomisp_3a_statistics *config)
2774 {
2775         struct atomisp_device *isp = asd->isp;
2776         struct atomisp_s3a_buf *s3a_buf;
2777         unsigned long ret;
2778
2779         if (flag != 0)
2780                 return -EINVAL;
2781
2782         /* sanity check to avoid writing into unallocated memory. */
2783         if (asd->params.s3a_output_bytes == 0)
2784                 return -EINVAL;
2785
2786         if (atomisp_compare_grid(asd, &config->grid_info) != 0) {
2787                 /* If the grid info in the argument differs from the current
2788                    grid info, we tell the caller to reset the grid size and
2789                    try again. */
2790                 return -EAGAIN;
2791         }
2792
2793         if (list_empty(&asd->s3a_stats_ready)) {
2794                 dev_err(isp->dev, "3a statistics is not valid.\n");
2795                 return -EAGAIN;
2796         }
2797
2798         s3a_buf = list_entry(asd->s3a_stats_ready.next,
2799                         struct atomisp_s3a_buf, list);
2800         if (s3a_buf->s3a_map)
2801                 ia_css_translate_3a_statistics(
2802                         asd->params.s3a_user_stat, s3a_buf->s3a_map);
2803         else
2804                 ia_css_get_3a_statistics(asd->params.s3a_user_stat,
2805                         s3a_buf->s3a_data);
2806
2807         config->exp_id = s3a_buf->s3a_data->exp_id;
2808         config->isp_config_id = s3a_buf->s3a_data->isp_config_id;
2809
2810         ret = copy_to_user(config->data, asd->params.s3a_user_stat->data,
2811                            asd->params.s3a_output_bytes);
2812         if (ret) {
2813                 dev_err(isp->dev, "copy to user failed: copied %lu bytes\n",
2814                                 ret);
2815                 return -EFAULT;
2816         }
2817
2818         /* Move to free buffer list */
2819         list_del_init(&s3a_buf->list);
2820         list_add_tail(&s3a_buf->list, &asd->s3a_stats);
2821         dev_dbg(isp->dev, "%s: finish getting exp_id %d 3a stat, isp_config_id %d\n", __func__,
2822                 config->exp_id, config->isp_config_id);
2823         return 0;
2824 }
2825
2826 int atomisp_get_metadata(struct atomisp_sub_device *asd, int flag,
2827                          struct atomisp_metadata *md)
2828 {
2829         struct atomisp_device *isp = asd->isp;
2830         struct ia_css_stream_config *stream_config;
2831         struct ia_css_stream_info *stream_info;
2832         struct camera_mipi_info *mipi_info;
2833         struct atomisp_metadata_buf *md_buf;
2834         enum atomisp_metadata_type md_type = ATOMISP_MAIN_METADATA;
2835         int ret, i;
2836
2837         if (flag != 0)
2838                 return -EINVAL;
2839
2840         stream_config = &asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].
2841                 stream_config;
2842         stream_info = &asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].
2843                 stream_info;
2844
2845         /* We always return the resolution and stride even if there is
2846          * no valid metadata. This allows the caller to get the information
2847          * needed to allocate user-space buffers. */
2848         md->width  = stream_info->metadata_info.resolution.width;
2849         md->height = stream_info->metadata_info.resolution.height;
2850         md->stride = stream_info->metadata_info.stride;
2851
2852         /* sanity check to avoid writing into unallocated memory.
2853          * This does not return an error because it is a valid way
2854          * for applications to detect that metadata is not enabled. */
2855         if (md->width == 0 || md->height == 0 || !md->data)
2856                 return 0;
2857
2858         /* This is done in the atomisp_buf_done() */
2859         if (list_empty(&asd->metadata_ready[md_type])) {
2860                 dev_warn(isp->dev, "Metadata queue is empty now!\n");
2861                 return -EAGAIN;
2862         }
2863
2864         mipi_info = atomisp_to_sensor_mipi_info(
2865                 isp->inputs[asd->input_curr].camera);
2866         if (mipi_info == NULL)
2867                 return -EINVAL;
2868
2869         if (mipi_info->metadata_effective_width != NULL) {
2870                 for (i = 0; i < md->height; i++)
2871                         md->effective_width[i] =
2872                                 mipi_info->metadata_effective_width[i];
2873         }
2874
2875         md_buf = list_entry(asd->metadata_ready[md_type].next,
2876                             struct atomisp_metadata_buf, list);
2877         md->exp_id = md_buf->metadata->exp_id;
2878         if (md_buf->md_vptr) {
2879                 ret = copy_to_user(md->data,
2880                                    md_buf->md_vptr,
2881                                    stream_info->metadata_info.size);
2882         } else {
2883                 hmm_load(md_buf->metadata->address,
2884                                     asd->params.metadata_user[md_type],
2885                                     stream_info->metadata_info.size);
2886
2887                 ret = copy_to_user(md->data,
2888                                    asd->params.metadata_user[md_type],
2889                                    stream_info->metadata_info.size);
2890         }
2891         if (ret) {
2892                 dev_err(isp->dev, "copy to user failed: copied %d bytes\n",
2893                         ret);
2894                 return -EFAULT;
2895         }
2896
2897         list_del_init(&md_buf->list);
2898         list_add_tail(&md_buf->list, &asd->metadata[md_type]);
2899
2900         dev_dbg(isp->dev, "%s: HAL de-queued metadata type %d with exp_id %d\n",
2901                 __func__, md_type, md->exp_id);
2902         return 0;
2903 }
2904
2905 int atomisp_get_metadata_by_type(struct atomisp_sub_device *asd, int flag,
2906                                  struct atomisp_metadata_with_type *md)
2907 {
2908         struct atomisp_device *isp = asd->isp;
2909         struct ia_css_stream_config *stream_config;
2910         struct ia_css_stream_info *stream_info;
2911         struct camera_mipi_info *mipi_info;
2912         struct atomisp_metadata_buf *md_buf;
2913         enum atomisp_metadata_type md_type;
2914         int ret, i;
2915
2916         if (flag != 0)
2917                 return -EINVAL;
2918
2919         stream_config = &asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].
2920                 stream_config;
2921         stream_info = &asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].
2922                 stream_info;
2923
2924         /* We always return the resolution and stride even if there is
2925          * no valid metadata. This allows the caller to get the information
2926          * needed to allocate user-space buffers. */
2927         md->width  = stream_info->metadata_info.resolution.width;
2928         md->height = stream_info->metadata_info.resolution.height;
2929         md->stride = stream_info->metadata_info.stride;
2930
2931         /* sanity check to avoid writing into unallocated memory.
2932          * This does not return an error because it is a valid way
2933          * for applications to detect that metadata is not enabled. */
2934         if (md->width == 0 || md->height == 0 || !md->data)
2935                 return 0;
2936
2937         md_type = md->type;
2938         if (md_type < 0 || md_type >= ATOMISP_METADATA_TYPE_NUM)
2939                 return -EINVAL;
2940
2941         /* This is done in the atomisp_buf_done() */
2942         if (list_empty(&asd->metadata_ready[md_type])) {
2943                 dev_warn(isp->dev, "Metadata queue is empty now!\n");
2944                 return -EAGAIN;
2945         }
2946
2947         mipi_info = atomisp_to_sensor_mipi_info(
2948                 isp->inputs[asd->input_curr].camera);
2949         if (mipi_info == NULL)
2950                 return -EINVAL;
2951
2952         if (mipi_info->metadata_effective_width != NULL) {
2953                 for (i = 0; i < md->height; i++)
2954                         md->effective_width[i] =
2955                                 mipi_info->metadata_effective_width[i];
2956         }
2957
2958         md_buf = list_entry(asd->metadata_ready[md_type].next,
2959                             struct atomisp_metadata_buf, list);
2960         md->exp_id = md_buf->metadata->exp_id;
2961         if (md_buf->md_vptr) {
2962                 ret = copy_to_user(md->data,
2963                                    md_buf->md_vptr,
2964                                    stream_info->metadata_info.size);
2965         } else {
2966                 hmm_load(md_buf->metadata->address,
2967                                     asd->params.metadata_user[md_type],
2968                                     stream_info->metadata_info.size);
2969
2970                 ret = copy_to_user(md->data,
2971                                    asd->params.metadata_user[md_type],
2972                                    stream_info->metadata_info.size);
2973         }
2974         if (ret) {
2975                 dev_err(isp->dev, "copy to user failed: copied %d bytes\n",
2976                         ret);
2977                 return -EFAULT;
2978         } else {
2979                 list_del_init(&md_buf->list);
2980                 list_add_tail(&md_buf->list, &asd->metadata[md_type]);
2981         }
2982         dev_dbg(isp->dev, "%s: HAL de-queued metadata type %d with exp_id %d\n",
2983                 __func__, md_type, md->exp_id);
2984         return 0;
2985 }
2986
2987 /*
2988  * Function to calculate real zoom region for every pipe
2989  */
2990 int atomisp_calculate_real_zoom_region(struct atomisp_sub_device *asd,
2991                                        struct ia_css_dz_config   *dz_config,
2992                                        enum atomisp_css_pipe_id css_pipe_id)
2993
2994 {
2995         struct atomisp_stream_env *stream_env =
2996                         &asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL];
2997         struct atomisp_resolution  eff_res, out_res;
2998 #ifdef ISP2401
2999         int w_offset, h_offset;
3000 #endif
3001
3002         memset(&eff_res, 0, sizeof(eff_res));
3003         memset(&out_res, 0, sizeof(out_res));
3004
3005         if (dz_config->dx || dz_config->dy)
3006                 return 0;
3007
3008         if (css_pipe_id != IA_CSS_PIPE_ID_PREVIEW
3009                 && css_pipe_id != IA_CSS_PIPE_ID_CAPTURE) {
3010                 dev_err(asd->isp->dev, "%s the set pipe no support crop region"
3011                         , __func__);
3012                 return -EINVAL;
3013         }
3014
3015         eff_res.width =
3016                 stream_env->stream_config.input_config.effective_res.width;
3017         eff_res.height =
3018                 stream_env->stream_config.input_config.effective_res.height;
3019         if (eff_res.width == 0 || eff_res.height == 0) {
3020                 dev_err(asd->isp->dev, "%s err effective resolution"
3021                                 , __func__);
3022                 return -EINVAL;
3023         }
3024
3025         if (dz_config->zoom_region.resolution.width
3026                 == asd->sensor_array_res.width
3027                 || dz_config->zoom_region.resolution.height
3028                 == asd->sensor_array_res.height) {
3029                 /*no need crop region*/
3030                 dz_config->zoom_region.origin.x = 0;
3031                 dz_config->zoom_region.origin.y = 0;
3032                 dz_config->zoom_region.resolution.width = eff_res.width;
3033                 dz_config->zoom_region.resolution.height = eff_res.height;
3034                 return 0;
3035         }
3036
3037         /* FIXME:
3038          * This is not the correct implementation with Google's definition, due
3039          * to firmware limitation.
3040          * map real crop region base on above calculating base max crop region.
3041          */
3042 #ifdef ISP2401
3043         out_res.width =
3044                 stream_env->pipe_configs[css_pipe_id].output_info[0].res.width;
3045         out_res.height =
3046                 stream_env->pipe_configs[css_pipe_id].output_info[0].res.height;
3047         if (out_res.width == 0 || out_res.height == 0) {
3048                 dev_err(asd->isp->dev, "%s err current pipe output resolution"
3049                                 , __func__);
3050                 return -EINVAL;
3051         }
3052
3053         if (asd->sensor_array_res.width * out_res.height
3054                         < out_res.width * asd->sensor_array_res.height) {
3055                 h_offset = asd->sensor_array_res.height -
3056                                 asd->sensor_array_res.width
3057                                 * out_res.height / out_res.width;
3058                 h_offset = h_offset / 2;
3059                 if (dz_config->zoom_region.origin.y < h_offset)
3060                         dz_config->zoom_region.origin.y = 0;
3061                 else
3062                         dz_config->zoom_region.origin.y =
3063                                 dz_config->zoom_region.origin.y - h_offset;
3064                 w_offset = 0;
3065         } else {
3066                 w_offset = asd->sensor_array_res.width -
3067                                 asd->sensor_array_res.height
3068                                 * out_res.width / out_res.height;
3069                 w_offset = w_offset / 2;
3070                 if (dz_config->zoom_region.origin.x < w_offset)
3071                         dz_config->zoom_region.origin.x = 0;
3072                 else
3073                         dz_config->zoom_region.origin.x =
3074                                 dz_config->zoom_region.origin.x - w_offset;
3075                 h_offset = 0;
3076         }
3077 #endif
3078         dz_config->zoom_region.origin.x =
3079                         dz_config->zoom_region.origin.x
3080                         * eff_res.width
3081 #ifndef ISP2401
3082                         / asd->sensor_array_res.width;
3083 #else
3084                         / (asd->sensor_array_res.width -
3085                         2 * w_offset);
3086 #endif
3087         dz_config->zoom_region.origin.y =
3088                         dz_config->zoom_region.origin.y
3089                         * eff_res.height
3090 #ifndef ISP2401
3091                         / asd->sensor_array_res.height;
3092 #else
3093                         / (asd->sensor_array_res.height -
3094                         2 * h_offset);
3095 #endif
3096         dz_config->zoom_region.resolution.width =
3097                         dz_config->zoom_region.resolution.width
3098                         * eff_res.width
3099 #ifndef ISP2401
3100                         / asd->sensor_array_res.width;
3101 #else
3102                         / (asd->sensor_array_res.width -
3103                         2 * w_offset);
3104 #endif
3105         dz_config->zoom_region.resolution.height =
3106                         dz_config->zoom_region.resolution.height
3107                         * eff_res.height
3108 #ifndef ISP2401
3109                         / asd->sensor_array_res.height;
3110 #else
3111                         / (asd->sensor_array_res.height -
3112                         2 * h_offset);
3113 #endif
3114
3115         /*
3116           * Set same ratio of crop region resolution and current pipe output
3117           * resolution
3118           */
3119 #ifndef ISP2401
3120         out_res.width =
3121                 stream_env->pipe_configs[css_pipe_id].output_info[0].res.width;
3122         out_res.height =
3123                 stream_env->pipe_configs[css_pipe_id].output_info[0].res.height;
3124         if (out_res.width == 0 || out_res.height == 0) {
3125                 dev_err(asd->isp->dev, "%s err current pipe output resolution"
3126                                 , __func__);
3127                 return -EINVAL;
3128         }
3129
3130 #endif
3131         if (out_res.width * dz_config->zoom_region.resolution.height
3132                 > dz_config->zoom_region.resolution.width * out_res.height) {
3133                 dz_config->zoom_region.resolution.height =
3134                                 dz_config->zoom_region.resolution.width
3135                                 * out_res.height / out_res.width;
3136         } else {
3137                 dz_config->zoom_region.resolution.width =
3138                                 dz_config->zoom_region.resolution.height
3139                                 * out_res.width / out_res.height;
3140         }
3141         dev_dbg(asd->isp->dev, "%s crop region:(%d,%d),(%d,%d) eff_res(%d, %d) array_size(%d,%d) out_res(%d, %d)\n",
3142                         __func__, dz_config->zoom_region.origin.x,
3143                         dz_config->zoom_region.origin.y,
3144                         dz_config->zoom_region.resolution.width,
3145                         dz_config->zoom_region.resolution.height,
3146                         eff_res.width, eff_res.height,
3147                         asd->sensor_array_res.width,
3148                         asd->sensor_array_res.height,
3149                         out_res.width, out_res.height);
3150
3151
3152         if ((dz_config->zoom_region.origin.x +
3153                 dz_config->zoom_region.resolution.width
3154                 > eff_res.width) ||
3155                 (dz_config->zoom_region.origin.y +
3156                 dz_config->zoom_region.resolution.height
3157                 > eff_res.height))
3158                 return -EINVAL;
3159
3160         return 0;
3161 }
3162
3163
3164 /*
3165  * Function to check the zoom region whether is effective
3166  */
3167 static bool atomisp_check_zoom_region(
3168                         struct atomisp_sub_device *asd,
3169                         struct ia_css_dz_config *dz_config)
3170 {
3171         struct atomisp_resolution  config;
3172         bool flag = false;
3173         unsigned int w , h;
3174
3175         memset(&config, 0, sizeof(struct atomisp_resolution));
3176
3177         if (dz_config->dx && dz_config->dy)
3178                 return true;
3179
3180         config.width = asd->sensor_array_res.width;
3181         config.height = asd->sensor_array_res.height;
3182         w = dz_config->zoom_region.origin.x +
3183                 dz_config->zoom_region.resolution.width;
3184         h = dz_config->zoom_region.origin.y +
3185                 dz_config->zoom_region.resolution.height;
3186
3187         if ((w <= config.width) && (h <= config.height) && w > 0 && h > 0)
3188                 flag = true;
3189         else
3190                 /* setting error zoom region */
3191                 dev_err(asd->isp->dev, "%s zoom region ERROR:dz_config:(%d,%d),(%d,%d)array_res(%d, %d)\n",
3192                         __func__, dz_config->zoom_region.origin.x,
3193                         dz_config->zoom_region.origin.y,
3194                         dz_config->zoom_region.resolution.width,
3195                         dz_config->zoom_region.resolution.height,
3196                         config.width, config.height);
3197
3198         return flag;
3199 }
3200
3201 void atomisp_apply_css_parameters(
3202                                 struct atomisp_sub_device *asd,
3203                                 struct atomisp_css_params *css_param)
3204 {
3205         if (css_param->update_flag.wb_config)
3206                 atomisp_css_set_wb_config(asd, &css_param->wb_config);
3207
3208         if (css_param->update_flag.ob_config)
3209                 atomisp_css_set_ob_config(asd, &css_param->ob_config);
3210
3211         if (css_param->update_flag.dp_config)
3212                 atomisp_css_set_dp_config(asd, &css_param->dp_config);
3213
3214         if (css_param->update_flag.nr_config)
3215                 atomisp_css_set_nr_config(asd, &css_param->nr_config);
3216
3217         if (css_param->update_flag.ee_config)
3218                 atomisp_css_set_ee_config(asd, &css_param->ee_config);
3219
3220         if (css_param->update_flag.tnr_config)
3221                 atomisp_css_set_tnr_config(asd, &css_param->tnr_config);
3222
3223         if (css_param->update_flag.a3a_config)
3224                 atomisp_css_set_3a_config(asd, &css_param->s3a_config);
3225
3226         if (css_param->update_flag.ctc_config)
3227                 atomisp_css_set_ctc_config(asd, &css_param->ctc_config);
3228
3229         if (css_param->update_flag.cnr_config)
3230                 atomisp_css_set_cnr_config(asd, &css_param->cnr_config);
3231
3232         if (css_param->update_flag.ecd_config)
3233                 atomisp_css_set_ecd_config(asd, &css_param->ecd_config);
3234
3235         if (css_param->update_flag.ynr_config)
3236                 atomisp_css_set_ynr_config(asd, &css_param->ynr_config);
3237
3238         if (css_param->update_flag.fc_config)
3239                 atomisp_css_set_fc_config(asd, &css_param->fc_config);
3240
3241         if (css_param->update_flag.macc_config)
3242                 atomisp_css_set_macc_config(asd, &css_param->macc_config);
3243
3244         if (css_param->update_flag.aa_config)
3245                 atomisp_css_set_aa_config(asd, &css_param->aa_config);
3246
3247         if (css_param->update_flag.anr_config)
3248                 atomisp_css_set_anr_config(asd, &css_param->anr_config);
3249
3250         if (css_param->update_flag.xnr_config)
3251                 atomisp_css_set_xnr_config(asd, &css_param->xnr_config);
3252
3253         if (css_param->update_flag.yuv2rgb_cc_config)
3254                 atomisp_css_set_yuv2rgb_cc_config(asd,
3255                                         &css_param->yuv2rgb_cc_config);
3256
3257         if (css_param->update_flag.rgb2yuv_cc_config)
3258                 atomisp_css_set_rgb2yuv_cc_config(asd,
3259                                         &css_param->rgb2yuv_cc_config);
3260
3261         if (css_param->update_flag.macc_table)
3262                 atomisp_css_set_macc_table(asd, &css_param->macc_table);
3263
3264         if (css_param->update_flag.xnr_table)
3265                 atomisp_css_set_xnr_table(asd, &css_param->xnr_table);
3266
3267         if (css_param->update_flag.r_gamma_table)
3268                 atomisp_css_set_r_gamma_table(asd, &css_param->r_gamma_table);
3269
3270         if (css_param->update_flag.g_gamma_table)
3271                 atomisp_css_set_g_gamma_table(asd, &css_param->g_gamma_table);
3272
3273         if (css_param->update_flag.b_gamma_table)
3274                 atomisp_css_set_b_gamma_table(asd, &css_param->b_gamma_table);
3275
3276         if (css_param->update_flag.anr_thres)
3277                 atomisp_css_set_anr_thres(asd, &css_param->anr_thres);
3278
3279         if (css_param->update_flag.shading_table)
3280                 atomisp_css_set_shading_table(asd, css_param->shading_table);
3281
3282         if (css_param->update_flag.morph_table && asd->params.gdc_cac_en)
3283                 atomisp_css_set_morph_table(asd, css_param->morph_table);
3284
3285         if (css_param->update_flag.dvs2_coefs) {
3286                 struct atomisp_css_dvs_grid_info *dvs_grid_info =
3287                         atomisp_css_get_dvs_grid_info(
3288                                 &asd->params.curr_grid_info);
3289
3290                 if (dvs_grid_info && dvs_grid_info->enable)
3291                         atomisp_css_set_dvs2_coefs(asd, css_param->dvs2_coeff);
3292         }
3293
3294         if (css_param->update_flag.dvs_6axis_config)
3295                 atomisp_css_set_dvs_6axis(asd, css_param->dvs_6axis);
3296
3297         atomisp_css_set_isp_config_id(asd, css_param->isp_config_id);
3298         /*
3299          * These configurations are on used by ISP1.x, not for ISP2.x,
3300          * so do not handle them. see comments of ia_css_isp_config.
3301          * 1 cc_config
3302          * 2 ce_config
3303          * 3 de_config
3304          * 4 gc_config
3305          * 5 gamma_table
3306          * 6 ctc_table
3307          * 7 dvs_coefs
3308          */
3309 }
3310
3311 static unsigned int long copy_from_compatible(void *to, const void *from,
3312                                               unsigned long n, bool from_user)
3313 {
3314         if (from_user)
3315                 return copy_from_user(to, from, n);
3316         else
3317                 memcpy(to, from, n);
3318         return 0;
3319 }
3320
3321 int atomisp_cp_general_isp_parameters(struct atomisp_sub_device *asd,
3322                                       struct atomisp_parameters *arg,
3323                                       struct atomisp_css_params *css_param,
3324                                       bool from_user)
3325 {
3326         struct atomisp_parameters *cur_config = &css_param->update_flag;
3327
3328         if (!arg || !asd || !css_param)
3329                 return -EINVAL;
3330
3331         if (arg->wb_config && (from_user || !cur_config->wb_config)) {
3332                 if (copy_from_compatible(&css_param->wb_config, arg->wb_config,
3333                                 sizeof(struct atomisp_css_wb_config),
3334                                 from_user))
3335                         return -EFAULT;
3336                 css_param->update_flag.wb_config =
3337                         (struct atomisp_wb_config *) &css_param->wb_config;
3338         }
3339
3340         if (arg->ob_config && (from_user || !cur_config->ob_config)) {
3341                 if (copy_from_compatible(&css_param->ob_config, arg->ob_config,
3342                                 sizeof(struct atomisp_css_ob_config),
3343                                 from_user))
3344                         return -EFAULT;
3345                 css_param->update_flag.ob_config =
3346                         (struct atomisp_ob_config *) &css_param->ob_config;
3347         }
3348
3349         if (arg->dp_config && (from_user || !cur_config->dp_config)) {
3350                 if (copy_from_compatible(&css_param->dp_config, arg->dp_config,
3351                                 sizeof(struct atomisp_css_dp_config),
3352                                 from_user))
3353                         return -EFAULT;
3354                 css_param->update_flag.dp_config =
3355                         (struct atomisp_dp_config *) &css_param->dp_config;
3356         }
3357
3358         if (asd->run_mode->val != ATOMISP_RUN_MODE_VIDEO) {
3359                 if (arg->dz_config && (from_user || !cur_config->dz_config)) {
3360                         if (copy_from_compatible(&css_param->dz_config,
3361                                 arg->dz_config,
3362                                 sizeof(struct atomisp_css_dz_config),
3363                                 from_user))
3364                                 return -EFAULT;
3365                         if (!atomisp_check_zoom_region(asd,
3366                                                 &css_param->dz_config)) {
3367                                 dev_err(asd->isp->dev, "crop region error!");
3368                                 return -EINVAL;
3369                         }
3370                         css_param->update_flag.dz_config =
3371                                 (struct atomisp_dz_config *)
3372                                 &css_param->dz_config;
3373                 }
3374         }
3375
3376         if (arg->nr_config && (from_user || !cur_config->nr_config)) {
3377                 if (copy_from_compatible(&css_param->nr_config, arg->nr_config,
3378                                 sizeof(struct atomisp_css_nr_config),
3379                                 from_user))
3380                         return -EFAULT;
3381                 css_param->update_flag.nr_config =
3382                         (struct atomisp_nr_config *) &css_param->nr_config;
3383         }
3384
3385         if (arg->ee_config && (from_user || !cur_config->ee_config)) {
3386                 if (copy_from_compatible(&css_param->ee_config, arg->ee_config,
3387                                 sizeof(struct atomisp_css_ee_config),
3388                                 from_user))
3389                         return -EFAULT;
3390                 css_param->update_flag.ee_config =
3391                         (struct atomisp_ee_config *) &css_param->ee_config;
3392         }
3393
3394         if (arg->tnr_config && (from_user || !cur_config->tnr_config)) {
3395                 if (copy_from_compatible(&css_param->tnr_config,
3396                                 arg->tnr_config,
3397                                 sizeof(struct atomisp_css_tnr_config),
3398                                 from_user))
3399                         return -EFAULT;
3400                 css_param->update_flag.tnr_config =
3401                         (struct atomisp_tnr_config *)
3402                         &css_param->tnr_config;
3403         }
3404
3405         if (arg->a3a_config && (from_user || !cur_config->a3a_config)) {
3406                 if (copy_from_compatible(&css_param->s3a_config,
3407                                 arg->a3a_config,
3408                                 sizeof(struct atomisp_css_3a_config),
3409                                 from_user))
3410                         return -EFAULT;
3411                 css_param->update_flag.a3a_config =
3412                         (struct atomisp_3a_config *) &css_param->s3a_config;
3413         }
3414
3415         if (arg->ctc_config && (from_user || !cur_config->ctc_config)) {
3416                 if (copy_from_compatible(&css_param->ctc_config,
3417                                 arg->ctc_config,
3418                                 sizeof(struct atomisp_css_ctc_config),
3419                                 from_user))
3420                         return -EFAULT;
3421                 css_param->update_flag.ctc_config =
3422                         (struct atomisp_ctc_config *)
3423                         &css_param->ctc_config;
3424         }
3425
3426         if (arg->cnr_config && (from_user || !cur_config->cnr_config)) {
3427                 if (copy_from_compatible(&css_param->cnr_config,
3428                                 arg->cnr_config,
3429                                 sizeof(struct atomisp_css_cnr_config),
3430                                 from_user))
3431                         return -EFAULT;
3432                 css_param->update_flag.cnr_config =
3433                         (struct atomisp_cnr_config *)
3434                         &css_param->cnr_config;
3435         }
3436
3437         if (arg->ecd_config && (from_user || !cur_config->ecd_config)) {
3438                 if (copy_from_compatible(&css_param->ecd_config,
3439                                 arg->ecd_config,
3440                                 sizeof(struct atomisp_css_ecd_config),
3441                                 from_user))
3442                         return -EFAULT;
3443                 css_param->update_flag.ecd_config =
3444                         (struct atomisp_ecd_config *)
3445                         &css_param->ecd_config;
3446         }
3447
3448         if (arg->ynr_config && (from_user || !cur_config->ynr_config)) {
3449                 if (copy_from_compatible(&css_param->ynr_config,
3450                                 arg->ynr_config,
3451                                 sizeof(struct atomisp_css_ynr_config),
3452                                 from_user))
3453                         return -EFAULT;
3454                 css_param->update_flag.ynr_config =
3455                         (struct atomisp_ynr_config *)
3456                         &css_param->ynr_config;
3457         }
3458
3459         if (arg->fc_config && (from_user || !cur_config->fc_config)) {
3460                 if (copy_from_compatible(&css_param->fc_config,
3461                                 arg->fc_config,
3462                                 sizeof(struct atomisp_css_fc_config),
3463                                 from_user))
3464                         return -EFAULT;
3465                 css_param->update_flag.fc_config =
3466                         (struct atomisp_fc_config *) &css_param->fc_config;
3467         }
3468
3469         if (arg->macc_config && (from_user || !cur_config->macc_config)) {
3470                 if (copy_from_compatible(&css_param->macc_config,
3471                                 arg->macc_config,
3472                                 sizeof(struct atomisp_css_macc_config),
3473                                 from_user))
3474                         return -EFAULT;
3475                 css_param->update_flag.macc_config =
3476                         (struct atomisp_macc_config *)
3477                         &css_param->macc_config;
3478         }
3479
3480         if (arg->aa_config && (from_user || !cur_config->aa_config)) {
3481                 if (copy_from_compatible(&css_param->aa_config, arg->aa_config,
3482                                 sizeof(struct atomisp_css_aa_config),
3483                                 from_user))
3484                         return -EFAULT;
3485                 css_param->update_flag.aa_config =
3486                         (struct atomisp_aa_config *) &css_param->aa_config;
3487         }
3488
3489         if (arg->anr_config && (from_user || !cur_config->anr_config)) {
3490                 if (copy_from_compatible(&css_param->anr_config,
3491                                 arg->anr_config,
3492                                 sizeof(struct atomisp_css_anr_config),
3493                                 from_user))
3494                         return -EFAULT;
3495                 css_param->update_flag.anr_config =
3496                         (struct atomisp_anr_config *)
3497                         &css_param->anr_config;
3498         }
3499
3500         if (arg->xnr_config && (from_user || !cur_config->xnr_config)) {
3501                 if (copy_from_compatible(&css_param->xnr_config,
3502                                 arg->xnr_config,
3503                                 sizeof(struct atomisp_css_xnr_config),
3504                                 from_user))
3505                         return -EFAULT;
3506                 css_param->update_flag.xnr_config =
3507                         (struct atomisp_xnr_config *)
3508                         &css_param->xnr_config;
3509         }
3510
3511         if (arg->yuv2rgb_cc_config &&
3512            (from_user || !cur_config->yuv2rgb_cc_config)) {
3513                 if (copy_from_compatible(&css_param->yuv2rgb_cc_config,
3514                                 arg->yuv2rgb_cc_config,
3515                                 sizeof(struct atomisp_css_cc_config),
3516                                 from_user))
3517                         return -EFAULT;
3518                 css_param->update_flag.yuv2rgb_cc_config =
3519                         (struct atomisp_cc_config *)
3520                         &css_param->yuv2rgb_cc_config;
3521         }
3522
3523         if (arg->rgb2yuv_cc_config &&
3524            (from_user || !cur_config->rgb2yuv_cc_config)) {
3525                 if (copy_from_compatible(&css_param->rgb2yuv_cc_config,
3526                                 arg->rgb2yuv_cc_config,
3527                                 sizeof(struct atomisp_css_cc_config),
3528                                 from_user))
3529                         return -EFAULT;
3530                 css_param->update_flag.rgb2yuv_cc_config =
3531                         (struct atomisp_cc_config *)
3532                         &css_param->rgb2yuv_cc_config;
3533         }
3534
3535         if (arg->macc_table && (from_user || !cur_config->macc_table)) {
3536                 if (copy_from_compatible(&css_param->macc_table,
3537                                 arg->macc_table,
3538                                 sizeof(struct atomisp_css_macc_table),
3539                                 from_user))
3540                         return -EFAULT;
3541                 css_param->update_flag.macc_table =
3542                         (struct atomisp_macc_table *)
3543                         &css_param->macc_table;
3544         }
3545
3546         if (arg->xnr_table && (from_user || !cur_config->xnr_table)) {
3547                 if (copy_from_compatible(&css_param->xnr_table,
3548                                 arg->xnr_table,
3549                                 sizeof(struct atomisp_css_xnr_table),
3550                                 from_user))
3551                         return -EFAULT;
3552                 css_param->update_flag.xnr_table =
3553                         (struct atomisp_xnr_table *) &css_param->xnr_table;
3554         }
3555
3556         if (arg->r_gamma_table && (from_user || !cur_config->r_gamma_table)) {
3557                 if (copy_from_compatible(&css_param->r_gamma_table,
3558                                 arg->r_gamma_table,
3559                                 sizeof(struct atomisp_css_rgb_gamma_table),
3560                                 from_user))
3561                         return -EFAULT;
3562                 css_param->update_flag.r_gamma_table =
3563                         (struct atomisp_rgb_gamma_table *)
3564                         &css_param->r_gamma_table;
3565         }
3566
3567         if (arg->g_gamma_table && (from_user || !cur_config->g_gamma_table)) {
3568                 if (copy_from_compatible(&css_param->g_gamma_table,
3569                                 arg->g_gamma_table,
3570                                 sizeof(struct atomisp_css_rgb_gamma_table),
3571                                 from_user))
3572                         return -EFAULT;
3573                 css_param->update_flag.g_gamma_table =
3574                         (struct atomisp_rgb_gamma_table *)
3575                         &css_param->g_gamma_table;
3576         }
3577
3578         if (arg->b_gamma_table && (from_user || !cur_config->b_gamma_table)) {
3579                 if (copy_from_compatible(&css_param->b_gamma_table,
3580                                 arg->b_gamma_table,
3581                                 sizeof(struct atomisp_css_rgb_gamma_table),
3582                                 from_user))
3583                         return -EFAULT;
3584                 css_param->update_flag.b_gamma_table =
3585                         (struct atomisp_rgb_gamma_table *)
3586                         &css_param->b_gamma_table;
3587         }
3588
3589         if (arg->anr_thres && (from_user || !cur_config->anr_thres)) {
3590                 if (copy_from_compatible(&css_param->anr_thres, arg->anr_thres,
3591                                 sizeof(struct atomisp_css_anr_thres),
3592                                 from_user))
3593                         return -EFAULT;
3594                 css_param->update_flag.anr_thres =
3595                         (struct atomisp_anr_thres *) &css_param->anr_thres;
3596         }
3597
3598         if (from_user)
3599                 css_param->isp_config_id = arg->isp_config_id;
3600         /*
3601          * These configurations are on used by ISP1.x, not for ISP2.x,
3602          * so do not handle them. see comments of ia_css_isp_config.
3603          * 1 cc_config
3604          * 2 ce_config
3605          * 3 de_config
3606          * 4 gc_config
3607          * 5 gamma_table
3608          * 6 ctc_table
3609          * 7 dvs_coefs
3610          */
3611         return 0;
3612 }
3613
3614 int atomisp_cp_lsc_table(struct atomisp_sub_device *asd,
3615                          struct atomisp_shading_table *source_st,
3616                          struct atomisp_css_params *css_param,
3617                          bool from_user)
3618 {
3619         unsigned int i;
3620         unsigned int len_table;
3621         struct atomisp_css_shading_table *shading_table;
3622         struct atomisp_css_shading_table *old_table;
3623 #ifdef ISP2401
3624         struct atomisp_shading_table st;
3625 #endif
3626
3627         if (!source_st)
3628                 return 0;
3629
3630         if (!css_param)
3631                 return -EINVAL;
3632
3633         if (!from_user && css_param->update_flag.shading_table)
3634                 return 0;
3635
3636 #ifdef ISP2401
3637         if (copy_from_compatible(&st, source_st,
3638                                  sizeof(struct atomisp_shading_table),
3639                                  from_user)) {
3640                 dev_err(asd->isp->dev, "copy shading table failed!");
3641                 return -EFAULT;
3642         }
3643
3644 #endif
3645         old_table = css_param->shading_table;
3646
3647 #ifdef ISP2401
3648
3649 #endif
3650         /* user config is to disable the shading table. */
3651 #ifndef ISP2401
3652         if (!source_st->enable) {
3653 #else
3654         if (!st.enable) {
3655 #endif
3656                 /* Generate a minimum table with enable = 0. */
3657                 shading_table = atomisp_css_shading_table_alloc(1, 1);
3658                 if (!shading_table)
3659                         return -ENOMEM;
3660                 shading_table->enable = 0;
3661                 goto set_lsc;
3662         }
3663
3664         /* Setting a new table. Validate first - all tables must be set */
3665         for (i = 0; i < ATOMISP_NUM_SC_COLORS; i++) {
3666 #ifndef ISP2401
3667                 if (!source_st->data[i])
3668 #else
3669                 if (!st.data[i]) {
3670                         dev_err(asd->isp->dev, "shading table validate failed");
3671 #endif
3672                         return -EINVAL;
3673 #ifdef ISP2401
3674                 }
3675 #endif
3676         }
3677
3678         /* Shading table size per color */
3679 #ifndef ISP2401
3680         if (source_st->width > SH_CSS_MAX_SCTBL_WIDTH_PER_COLOR ||
3681                 source_st->height > SH_CSS_MAX_SCTBL_HEIGHT_PER_COLOR)
3682 #else
3683         if (st.width > SH_CSS_MAX_SCTBL_WIDTH_PER_COLOR ||
3684             st.height > SH_CSS_MAX_SCTBL_HEIGHT_PER_COLOR) {
3685                 dev_err(asd->isp->dev, "shading table w/h validate failed!");
3686 #endif
3687                 return -EINVAL;
3688 #ifdef ISP2401
3689         }
3690 #endif
3691
3692 #ifndef ISP2401
3693         shading_table = atomisp_css_shading_table_alloc(source_st->width,
3694                                                         source_st->height);
3695         if (!shading_table)
3696                         return -ENOMEM;
3697 #else
3698         shading_table = atomisp_css_shading_table_alloc(st.width,
3699                                                         st.height);
3700         if (!shading_table) {
3701                 dev_err(asd->isp->dev, "shading table alloc failed!");
3702                 return -ENOMEM;
3703         }
3704 #endif
3705
3706 #ifndef ISP2401
3707         len_table = source_st->width * source_st->height * ATOMISP_SC_TYPE_SIZE;
3708 #else
3709         len_table = st.width * st.height * ATOMISP_SC_TYPE_SIZE;
3710 #endif
3711         for (i = 0; i < ATOMISP_NUM_SC_COLORS; i++) {
3712                 if (copy_from_compatible(shading_table->data[i],
3713 #ifndef ISP2401
3714                         source_st->data[i], len_table, from_user)) {
3715 #else
3716                         st.data[i], len_table, from_user)) {
3717 #endif
3718                         atomisp_css_shading_table_free(shading_table);
3719                         return -EFAULT;
3720                 }
3721
3722         }
3723 #ifndef ISP2401
3724         shading_table->sensor_width = source_st->sensor_width;
3725         shading_table->sensor_height = source_st->sensor_height;
3726         shading_table->fraction_bits = source_st->fraction_bits;
3727         shading_table->enable = source_st->enable;
3728 #else
3729         shading_table->sensor_width = st.sensor_width;
3730         shading_table->sensor_height = st.sensor_height;
3731         shading_table->fraction_bits = st.fraction_bits;
3732         shading_table->enable = st.enable;
3733 #endif
3734
3735         /* No need to update shading table if it is the same */
3736         if (old_table != NULL &&
3737                 old_table->sensor_width == shading_table->sensor_width &&
3738                 old_table->sensor_height == shading_table->sensor_height &&
3739                 old_table->width == shading_table->width &&
3740                 old_table->height == shading_table->height &&
3741                 old_table->fraction_bits == shading_table->fraction_bits &&
3742                 old_table->enable == shading_table->enable) {
3743                 bool data_is_same = true;
3744
3745                 for (i = 0; i < ATOMISP_NUM_SC_COLORS; i++) {
3746                         if (memcmp(shading_table->data[i], old_table->data[i],
3747                                    len_table) != 0) {
3748                                 data_is_same = false;
3749                                 break;
3750                         }
3751                 }
3752
3753                 if (data_is_same) {
3754                         atomisp_css_shading_table_free(shading_table);
3755                         return 0;
3756                 }
3757         }
3758
3759 set_lsc:
3760         /* set LSC to CSS */
3761         css_param->shading_table = shading_table;
3762         css_param->update_flag.shading_table =
3763                 (struct atomisp_shading_table *) shading_table;
3764         asd->params.sc_en = shading_table != NULL;
3765
3766         if (old_table)
3767                 atomisp_css_shading_table_free(old_table);
3768
3769         return 0;
3770 }
3771
3772 int atomisp_css_cp_dvs2_coefs(struct atomisp_sub_device *asd,
3773                               struct ia_css_dvs2_coefficients *coefs,
3774                               struct atomisp_css_params *css_param,
3775                               bool from_user)
3776 {
3777         struct atomisp_css_dvs_grid_info *cur =
3778                 atomisp_css_get_dvs_grid_info(&asd->params.curr_grid_info);
3779         int dvs_hor_coef_bytes, dvs_ver_coef_bytes;
3780 #ifdef ISP2401
3781         struct ia_css_dvs2_coefficients dvs2_coefs;
3782 #endif
3783
3784         if (!coefs || !cur)
3785                 return 0;
3786
3787         if (!from_user && css_param->update_flag.dvs2_coefs)
3788                 return 0;
3789
3790 #ifndef ISP2401
3791         if (sizeof(*cur) != sizeof(coefs->grid) ||
3792             memcmp(&coefs->grid, cur, sizeof(coefs->grid))) {
3793 #else
3794         if (copy_from_compatible(&dvs2_coefs, coefs,
3795                                  sizeof(struct ia_css_dvs2_coefficients),
3796                                  from_user)) {
3797                 dev_err(asd->isp->dev, "copy dvs2 coef failed");
3798                 return -EFAULT;
3799         }
3800
3801         if (sizeof(*cur) != sizeof(dvs2_coefs.grid) ||
3802             memcmp(&dvs2_coefs.grid, cur, sizeof(dvs2_coefs.grid))) {
3803 #endif
3804                 dev_err(asd->isp->dev, "dvs grid mis-match!\n");
3805                 /* If the grid info in the argument differs from the current
3806                    grid info, we tell the caller to reset the grid size and
3807                    try again. */
3808                 return -EAGAIN;
3809         }
3810
3811 #ifndef ISP2401
3812         if (coefs->hor_coefs.odd_real == NULL ||
3813             coefs->hor_coefs.odd_imag == NULL ||
3814             coefs->hor_coefs.even_real == NULL ||
3815             coefs->hor_coefs.even_imag == NULL ||
3816             coefs->ver_coefs.odd_real == NULL ||
3817             coefs->ver_coefs.odd_imag == NULL ||
3818             coefs->ver_coefs.even_real == NULL ||
3819             coefs->ver_coefs.even_imag == NULL)
3820 #else
3821         if (dvs2_coefs.hor_coefs.odd_real == NULL ||
3822             dvs2_coefs.hor_coefs.odd_imag == NULL ||
3823             dvs2_coefs.hor_coefs.even_real == NULL ||
3824             dvs2_coefs.hor_coefs.even_imag == NULL ||
3825             dvs2_coefs.ver_coefs.odd_real == NULL ||
3826             dvs2_coefs.ver_coefs.odd_imag == NULL ||
3827             dvs2_coefs.ver_coefs.even_real == NULL ||
3828             dvs2_coefs.ver_coefs.even_imag == NULL)
3829 #endif
3830                 return -EINVAL;
3831
3832         if (!css_param->dvs2_coeff) {
3833                 /* DIS coefficients. */
3834                 css_param->dvs2_coeff = ia_css_dvs2_coefficients_allocate(cur);
3835                 if (!css_param->dvs2_coeff)
3836                         return -ENOMEM;
3837         }
3838
3839         dvs_hor_coef_bytes = asd->params.dvs_hor_coef_bytes;
3840         dvs_ver_coef_bytes = asd->params.dvs_ver_coef_bytes;
3841         if (copy_from_compatible(css_param->dvs2_coeff->hor_coefs.odd_real,
3842 #ifndef ISP2401
3843             coefs->hor_coefs.odd_real, dvs_hor_coef_bytes, from_user) ||
3844 #else
3845             dvs2_coefs.hor_coefs.odd_real, dvs_hor_coef_bytes, from_user) ||
3846 #endif
3847             copy_from_compatible(css_param->dvs2_coeff->hor_coefs.odd_imag,
3848 #ifndef ISP2401
3849             coefs->hor_coefs.odd_imag, dvs_hor_coef_bytes, from_user) ||
3850 #else
3851             dvs2_coefs.hor_coefs.odd_imag, dvs_hor_coef_bytes, from_user) ||
3852 #endif
3853             copy_from_compatible(css_param->dvs2_coeff->hor_coefs.even_real,
3854 #ifndef ISP2401
3855             coefs->hor_coefs.even_real, dvs_hor_coef_bytes, from_user) ||
3856 #else
3857             dvs2_coefs.hor_coefs.even_real, dvs_hor_coef_bytes, from_user) ||
3858 #endif
3859             copy_from_compatible(css_param->dvs2_coeff->hor_coefs.even_imag,
3860 #ifndef ISP2401
3861             coefs->hor_coefs.even_imag, dvs_hor_coef_bytes, from_user) ||
3862 #else
3863             dvs2_coefs.hor_coefs.even_imag, dvs_hor_coef_bytes, from_user) ||
3864 #endif
3865             copy_from_compatible(css_param->dvs2_coeff->ver_coefs.odd_real,
3866 #ifndef ISP2401
3867             coefs->ver_coefs.odd_real, dvs_ver_coef_bytes, from_user) ||
3868 #else
3869             dvs2_coefs.ver_coefs.odd_real, dvs_ver_coef_bytes, from_user) ||
3870 #endif
3871             copy_from_compatible(css_param->dvs2_coeff->ver_coefs.odd_imag,
3872 #ifndef ISP2401
3873             coefs->ver_coefs.odd_imag, dvs_ver_coef_bytes, from_user) ||
3874 #else
3875             dvs2_coefs.ver_coefs.odd_imag, dvs_ver_coef_bytes, from_user) ||
3876 #endif
3877             copy_from_compatible(css_param->dvs2_coeff->ver_coefs.even_real,
3878 #ifndef ISP2401
3879             coefs->ver_coefs.even_real, dvs_ver_coef_bytes, from_user) ||
3880 #else
3881             dvs2_coefs.ver_coefs.even_real, dvs_ver_coef_bytes, from_user) ||
3882 #endif
3883             copy_from_compatible(css_param->dvs2_coeff->ver_coefs.even_imag,
3884 #ifndef ISP2401
3885             coefs->ver_coefs.even_imag, dvs_ver_coef_bytes, from_user)) {
3886 #else
3887             dvs2_coefs.ver_coefs.even_imag, dvs_ver_coef_bytes, from_user)) {
3888 #endif
3889                 ia_css_dvs2_coefficients_free(css_param->dvs2_coeff);
3890                 css_param->dvs2_coeff = NULL;
3891                 return -EFAULT;
3892         }
3893
3894         css_param->update_flag.dvs2_coefs =
3895             (struct atomisp_dvs2_coefficients *)css_param->dvs2_coeff;
3896         return 0;
3897 }
3898
3899 int atomisp_cp_dvs_6axis_config(struct atomisp_sub_device *asd,
3900                         struct atomisp_dvs_6axis_config *source_6axis_config,
3901                         struct atomisp_css_params *css_param,
3902                         bool from_user)
3903 {
3904         struct atomisp_css_dvs_6axis_config *dvs_6axis_config;
3905         struct atomisp_css_dvs_6axis_config *old_6axis_config;
3906 #ifdef ISP2401
3907         struct atomisp_css_dvs_6axis_config t_6axis_config;
3908 #endif
3909         struct ia_css_stream *stream =
3910                         asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].stream;
3911         struct atomisp_css_dvs_grid_info *dvs_grid_info =
3912                 atomisp_css_get_dvs_grid_info(&asd->params.curr_grid_info);
3913         int ret = -EFAULT;
3914
3915         if (stream == NULL) {
3916                 dev_err(asd->isp->dev, "%s: internal error!", __func__);
3917                 return -EINVAL;
3918         }
3919
3920         if (!source_6axis_config || !dvs_grid_info)
3921                 return 0;
3922
3923         if (!dvs_grid_info->enable)
3924                 return 0;
3925
3926         if (!from_user && css_param->update_flag.dvs_6axis_config)
3927                 return 0;
3928
3929         /* check whether need to reallocate for 6 axis config */
3930         old_6axis_config = css_param->dvs_6axis;
3931         dvs_6axis_config = old_6axis_config;
3932 #ifdef ISP2401
3933
3934         if (copy_from_compatible(&t_6axis_config, source_6axis_config,
3935                            sizeof(struct atomisp_dvs_6axis_config),
3936                            from_user)) {
3937                 dev_err(asd->isp->dev, "copy morph table failed!");
3938                 return -EFAULT;
3939         }
3940
3941 #endif
3942         if (old_6axis_config &&
3943 #ifndef ISP2401
3944             (old_6axis_config->width_y != source_6axis_config->width_y ||
3945              old_6axis_config->height_y != source_6axis_config->height_y ||
3946              old_6axis_config->width_uv != source_6axis_config->width_uv ||
3947              old_6axis_config->height_uv != source_6axis_config->height_uv)) {
3948 #else
3949             (old_6axis_config->width_y != t_6axis_config.width_y ||
3950              old_6axis_config->height_y != t_6axis_config.height_y ||
3951              old_6axis_config->width_uv != t_6axis_config.width_uv ||
3952              old_6axis_config->height_uv != t_6axis_config.height_uv)) {
3953 #endif
3954                 ia_css_dvs2_6axis_config_free(css_param->dvs_6axis);
3955                 css_param->dvs_6axis = NULL;
3956
3957                 dvs_6axis_config = ia_css_dvs2_6axis_config_allocate(stream);
3958                 if (!dvs_6axis_config)
3959                         return -ENOMEM;
3960         } else if (!dvs_6axis_config) {
3961                 dvs_6axis_config = ia_css_dvs2_6axis_config_allocate(stream);
3962                 if (!dvs_6axis_config)
3963                         return -ENOMEM;
3964         }
3965
3966 #ifndef ISP2401
3967         dvs_6axis_config->exp_id = source_6axis_config->exp_id;
3968 #else
3969         dvs_6axis_config->exp_id = t_6axis_config.exp_id;
3970 #endif
3971
3972         if (copy_from_compatible(dvs_6axis_config->xcoords_y,
3973 #ifndef ISP2401
3974                            source_6axis_config->xcoords_y,
3975                            source_6axis_config->width_y *
3976                            source_6axis_config->height_y *
3977                            sizeof(*source_6axis_config->xcoords_y),
3978 #else
3979                            t_6axis_config.xcoords_y,
3980                            t_6axis_config.width_y *
3981                            t_6axis_config.height_y *
3982                            sizeof(*dvs_6axis_config->xcoords_y),
3983 #endif
3984                            from_user))
3985                 goto error;
3986         if (copy_from_compatible(dvs_6axis_config->ycoords_y,
3987 #ifndef ISP2401
3988                            source_6axis_config->ycoords_y,
3989                            source_6axis_config->width_y *
3990                            source_6axis_config->height_y *
3991                            sizeof(*source_6axis_config->ycoords_y),
3992 #else
3993                            t_6axis_config.ycoords_y,
3994                            t_6axis_config.width_y *
3995                            t_6axis_config.height_y *
3996                            sizeof(*dvs_6axis_config->ycoords_y),
3997 #endif
3998                            from_user))
3999                 goto error;
4000         if (copy_from_compatible(dvs_6axis_config->xcoords_uv,
4001 #ifndef ISP2401
4002                            source_6axis_config->xcoords_uv,
4003                            source_6axis_config->width_uv *
4004                            source_6axis_config->height_uv *
4005                            sizeof(*source_6axis_config->xcoords_uv),
4006 #else
4007                            t_6axis_config.xcoords_uv,
4008                            t_6axis_config.width_uv *
4009                            t_6axis_config.height_uv *
4010                            sizeof(*dvs_6axis_config->xcoords_uv),
4011 #endif
4012                            from_user))
4013                 goto error;
4014         if (copy_from_compatible(dvs_6axis_config->ycoords_uv,
4015 #ifndef ISP2401
4016                            source_6axis_config->ycoords_uv,
4017                            source_6axis_config->width_uv *
4018                            source_6axis_config->height_uv *
4019                            sizeof(*source_6axis_config->ycoords_uv),
4020 #else
4021                            t_6axis_config.ycoords_uv,
4022                            t_6axis_config.width_uv *
4023                            t_6axis_config.height_uv *
4024                            sizeof(*dvs_6axis_config->ycoords_uv),
4025 #endif
4026                            from_user))
4027                 goto error;
4028
4029         css_param->dvs_6axis = dvs_6axis_config;
4030         css_param->update_flag.dvs_6axis_config =
4031                 (struct atomisp_dvs_6axis_config *) dvs_6axis_config;
4032         return 0;
4033
4034 error:
4035         if (dvs_6axis_config)
4036                 ia_css_dvs2_6axis_config_free(dvs_6axis_config);
4037         return ret;
4038 }
4039
4040 int atomisp_cp_morph_table(struct atomisp_sub_device *asd,
4041                                 struct atomisp_morph_table *source_morph_table,
4042                                 struct atomisp_css_params *css_param,
4043                                 bool from_user)
4044 {
4045         int ret = -EFAULT;
4046         unsigned int i;
4047         struct atomisp_css_morph_table *morph_table;
4048 #ifdef ISP2401
4049         struct atomisp_css_morph_table mtbl;
4050 #endif
4051         struct atomisp_css_morph_table *old_morph_table;
4052
4053         if (!source_morph_table)
4054                 return 0;
4055
4056         if (!from_user && css_param->update_flag.morph_table)
4057                 return 0;
4058
4059         old_morph_table = css_param->morph_table;
4060
4061 #ifdef ISP2401
4062         if (copy_from_compatible(&mtbl, source_morph_table,
4063                                  sizeof(struct atomisp_morph_table),
4064                                  from_user)) {
4065                 dev_err(asd->isp->dev, "copy morph table failed!");
4066                 return -EFAULT;
4067         }
4068
4069 #endif
4070         morph_table = atomisp_css_morph_table_allocate(
4071 #ifndef ISP2401
4072                 source_morph_table->width,
4073                 source_morph_table->height);
4074 #else
4075                 mtbl.width,
4076                 mtbl.height);
4077 #endif
4078         if (!morph_table)
4079                 return -ENOMEM;
4080
4081         for (i = 0; i < CSS_MORPH_TABLE_NUM_PLANES; i++) {
4082                 if (copy_from_compatible(morph_table->coordinates_x[i],
4083                         source_morph_table->coordinates_x[i],
4084 #ifndef ISP2401
4085                         source_morph_table->height * source_morph_table->width *
4086                         sizeof(*source_morph_table->coordinates_x[i]),
4087 #else
4088                         mtbl.height * mtbl.width *
4089                         sizeof(*morph_table->coordinates_x[i]),
4090 #endif
4091                         from_user))
4092                         goto error;
4093
4094                 if (copy_from_compatible(morph_table->coordinates_y[i],
4095                         source_morph_table->coordinates_y[i],
4096 #ifndef ISP2401
4097                         source_morph_table->height * source_morph_table->width *
4098                         sizeof(*source_morph_table->coordinates_y[i]),
4099 #else
4100                         mtbl.height * mtbl.width *
4101                         sizeof(*morph_table->coordinates_y[i]),
4102 #endif
4103                         from_user))
4104                         goto error;
4105         }
4106
4107         css_param->morph_table = morph_table;
4108         if (old_morph_table)
4109                 atomisp_css_morph_table_free(old_morph_table);
4110         css_param->update_flag.morph_table =
4111                 (struct atomisp_morph_table *) morph_table;
4112         return 0;
4113
4114 error:
4115         if (morph_table)
4116                 atomisp_css_morph_table_free(morph_table);
4117         return ret;
4118 }
4119
4120 int atomisp_makeup_css_parameters(struct atomisp_sub_device *asd,
4121                                   struct atomisp_parameters *arg,
4122                                   struct atomisp_css_params *css_param)
4123 {
4124         int ret;
4125
4126         ret = atomisp_cp_general_isp_parameters(asd, arg, css_param, false);
4127         if (ret)
4128                 return ret;
4129         ret = atomisp_cp_lsc_table(asd, arg->shading_table, css_param, false);
4130         if (ret)
4131                 return ret;
4132         ret = atomisp_cp_morph_table(asd, arg->morph_table, css_param, false);
4133         if (ret)
4134                 return ret;
4135         ret = atomisp_css_cp_dvs2_coefs(asd,
4136                 (struct ia_css_dvs2_coefficients *) arg->dvs2_coefs,
4137                 css_param, false);
4138         if (ret)
4139                 return ret;
4140         ret = atomisp_cp_dvs_6axis_config(asd, arg->dvs_6axis_config,
4141                                           css_param, false);
4142         return ret;
4143 }
4144
4145 void atomisp_free_css_parameters(struct atomisp_css_params *css_param)
4146 {
4147         if (css_param->dvs_6axis) {
4148                 ia_css_dvs2_6axis_config_free(css_param->dvs_6axis);
4149                 css_param->dvs_6axis = NULL;
4150         }
4151         if (css_param->dvs2_coeff) {
4152                 ia_css_dvs2_coefficients_free(css_param->dvs2_coeff);
4153                 css_param->dvs2_coeff = NULL;
4154         }
4155         if (css_param->shading_table) {
4156                 ia_css_shading_table_free(css_param->shading_table);
4157                 css_param->shading_table = NULL;
4158         }
4159         if (css_param->morph_table) {
4160                 ia_css_morph_table_free(css_param->morph_table);
4161                 css_param->morph_table = NULL;
4162         }
4163 }
4164
4165 /*
4166  * Check parameter queue list and buffer queue list to find out if matched items
4167  * and then set parameter to CSS and enqueue buffer to CSS.
4168  * Of course, if the buffer in buffer waiting list is not bound to a per-frame
4169  * parameter, it will be enqueued into CSS as long as the per-frame setting
4170  * buffers before it get enqueued.
4171  */
4172 void atomisp_handle_parameter_and_buffer(struct atomisp_video_pipe *pipe)
4173 {
4174         struct atomisp_sub_device *asd = pipe->asd;
4175         struct videobuf_buffer *vb = NULL, *vb_tmp;
4176         struct atomisp_css_params_with_list *param = NULL, *param_tmp;
4177         struct videobuf_vmalloc_memory *vm_mem = NULL;
4178         unsigned long irqflags;
4179         bool need_to_enqueue_buffer = false;
4180
4181         if (atomisp_is_vf_pipe(pipe))
4182                 return;
4183
4184         /*
4185          * CSS/FW requires set parameter and enqueue buffer happen after ISP
4186          * is streamon.
4187          */
4188         if (asd->streaming != ATOMISP_DEVICE_STREAMING_ENABLED)
4189                 return;
4190
4191         if (list_empty(&pipe->per_frame_params) ||
4192             list_empty(&pipe->buffers_waiting_for_param))
4193                 return;
4194
4195         list_for_each_entry_safe(vb, vb_tmp,
4196                         &pipe->buffers_waiting_for_param, queue) {
4197                 if (pipe->frame_request_config_id[vb->i]) {
4198                         list_for_each_entry_safe(param, param_tmp,
4199                                 &pipe->per_frame_params, list) {
4200                                 if (pipe->frame_request_config_id[vb->i] !=
4201                                     param->params.isp_config_id)
4202                                         continue;
4203
4204                                 list_del(&param->list);
4205                                 list_del(&vb->queue);
4206                                 /*
4207                                  * clear the request config id as the buffer
4208                                  * will be handled and enqueued into CSS soon
4209                                  */
4210                                 pipe->frame_request_config_id[vb->i] = 0;
4211                                 pipe->frame_params[vb->i] = param;
4212                                 vm_mem = vb->priv;
4213                                 BUG_ON(!vm_mem);
4214                                 break;
4215                         }
4216
4217                         if (vm_mem) {
4218                                 spin_lock_irqsave(&pipe->irq_lock, irqflags);
4219                                 list_add_tail(&vb->queue, &pipe->activeq);
4220                                 spin_unlock_irqrestore(&pipe->irq_lock, irqflags);
4221                                 vm_mem = NULL;
4222                                 need_to_enqueue_buffer = true;
4223                         } else {
4224                                 /* The is the end, stop further loop */
4225                                 break;
4226                         }
4227                 } else {
4228                         list_del(&vb->queue);
4229                         pipe->frame_params[vb->i] = NULL;
4230                         spin_lock_irqsave(&pipe->irq_lock, irqflags);
4231                         list_add_tail(&vb->queue, &pipe->activeq);
4232                         spin_unlock_irqrestore(&pipe->irq_lock, irqflags);
4233                         need_to_enqueue_buffer = true;
4234                 }
4235         }
4236
4237         if (need_to_enqueue_buffer) {
4238                 atomisp_qbuffers_to_css(asd);
4239 #ifndef ISP2401
4240                 if (!atomisp_is_wdt_running(asd) && atomisp_buffers_queued(asd))
4241                         atomisp_wdt_start(asd);
4242 #else
4243                 if (atomisp_buffers_queued_pipe(pipe)) {
4244                         if (!atomisp_is_wdt_running(pipe))
4245                                 atomisp_wdt_start(pipe);
4246                         else
4247                                 atomisp_wdt_refresh_pipe(pipe,
4248                                         ATOMISP_WDT_KEEP_CURRENT_DELAY);
4249                 }
4250 #endif
4251         }
4252 }
4253
4254 /*
4255 * Function to configure ISP parameters
4256 */
4257 int atomisp_set_parameters(struct video_device *vdev,
4258                         struct atomisp_parameters *arg)
4259 {
4260         struct atomisp_video_pipe *pipe = atomisp_to_video_pipe(vdev);
4261         struct atomisp_sub_device *asd = pipe->asd;
4262         struct atomisp_css_params_with_list *param = NULL;
4263         struct atomisp_css_params *css_param = &asd->params.css_param;
4264         int ret;
4265
4266         if (asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].stream == NULL) {
4267                 dev_err(asd->isp->dev, "%s: internal error!\n", __func__);
4268                 return -EINVAL;
4269         }
4270
4271         dev_dbg(asd->isp->dev, "%s: set parameter(per_frame_setting %d) for asd%d with isp_config_id %d of %s\n",
4272                 __func__, arg->per_frame_setting, asd->index,
4273                 arg->isp_config_id, vdev->name);
4274 #ifdef ISP2401
4275
4276         if (atomisp_is_vf_pipe(pipe) && arg->per_frame_setting) {
4277                 dev_err(asd->isp->dev, "%s: vf pipe not support per_frame_setting",
4278                         __func__);
4279                 return -EINVAL;
4280         }
4281
4282 #endif
4283         if (arg->per_frame_setting && !atomisp_is_vf_pipe(pipe)) {
4284                 /*
4285                  * Per-frame setting enabled, we allocate a new paramter
4286                  * buffer to cache the parameters and only when frame buffers
4287                  * are ready, the parameters will be set to CSS.
4288                  * per-frame setting only works for the main output frame.
4289                  */
4290                 param = kvzalloc(sizeof(*param), GFP_KERNEL);
4291                 if (!param) {
4292                         dev_err(asd->isp->dev, "%s: failed to alloc params buffer\n",
4293                                 __func__);
4294                         return -ENOMEM;
4295                 }
4296                 css_param = &param->params;
4297         }
4298
4299         ret = atomisp_cp_general_isp_parameters(asd, arg, css_param, true);
4300         if (ret)
4301                 goto apply_parameter_failed;
4302
4303         ret = atomisp_cp_lsc_table(asd, arg->shading_table, css_param, true);
4304         if (ret)
4305                 goto apply_parameter_failed;
4306
4307         ret = atomisp_cp_morph_table(asd, arg->morph_table, css_param, true);
4308         if (ret)
4309                 goto apply_parameter_failed;
4310
4311         ret = atomisp_css_cp_dvs2_coefs(asd,
4312                 (struct ia_css_dvs2_coefficients *) arg->dvs2_coefs,
4313                 css_param, true);
4314         if (ret)
4315                 goto apply_parameter_failed;
4316
4317         ret = atomisp_cp_dvs_6axis_config(asd, arg->dvs_6axis_config,
4318                                           css_param, true);
4319         if (ret)
4320                 goto apply_parameter_failed;
4321
4322         if (!(arg->per_frame_setting && !atomisp_is_vf_pipe(pipe))) {
4323                 /* indicate to CSS that we have parameters to be updated */
4324                 asd->params.css_update_params_needed = true;
4325         } else {
4326                 list_add_tail(&param->list, &pipe->per_frame_params);
4327                 atomisp_handle_parameter_and_buffer(pipe);
4328         }
4329
4330         return 0;
4331
4332 apply_parameter_failed:
4333         if (css_param)
4334                 atomisp_free_css_parameters(css_param);
4335         if (param)
4336                 kvfree(param);
4337
4338         return ret;
4339 }
4340
4341 /*
4342  * Function to set/get isp parameters to isp
4343  */
4344 int atomisp_param(struct atomisp_sub_device *asd, int flag,
4345                   struct atomisp_parm *config)
4346 {
4347         struct atomisp_device *isp = asd->isp;
4348         struct ia_css_pipe_config *vp_cfg =
4349                 &asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].
4350                 pipe_configs[IA_CSS_PIPE_ID_VIDEO];
4351
4352         /* Read parameter for 3A binary info */
4353         if (flag == 0) {
4354                 struct atomisp_css_dvs_grid_info *dvs_grid_info =
4355                         atomisp_css_get_dvs_grid_info(
4356                                 &asd->params.curr_grid_info);
4357
4358                 if (&config->info == NULL) {
4359                         dev_err(isp->dev, "ERROR: NULL pointer in grid_info\n");
4360                         return -EINVAL;
4361                 }
4362                 atomisp_curr_user_grid_info(asd, &config->info);
4363
4364                 /* We always return the resolution and stride even if there is
4365                  * no valid metadata. This allows the caller to get the
4366                  * information needed to allocate user-space buffers. */
4367                 config->metadata_config.metadata_height = asd->
4368                         stream_env[ATOMISP_INPUT_STREAM_GENERAL].stream_info.
4369                         metadata_info.resolution.height;
4370                 config->metadata_config.metadata_stride = asd->
4371                         stream_env[ATOMISP_INPUT_STREAM_GENERAL].stream_info.
4372                         metadata_info.stride;
4373
4374                 /* update dvs grid info */
4375                 if (dvs_grid_info)
4376                         memcpy(&config->dvs_grid,
4377                                 dvs_grid_info,
4378                                 sizeof(struct atomisp_css_dvs_grid_info));
4379
4380                 if (asd->run_mode->val != ATOMISP_RUN_MODE_VIDEO) {
4381                         config->dvs_envelop.width = 0;
4382                         config->dvs_envelop.height = 0;
4383                         return 0;
4384                 }
4385
4386                 /* update dvs envelop info */
4387                 if (!asd->continuous_mode->val) {
4388                         config->dvs_envelop.width = vp_cfg->dvs_envelope.width;
4389                         config->dvs_envelop.height =
4390                                         vp_cfg->dvs_envelope.height;
4391                 } else {
4392                         unsigned int dvs_w, dvs_h, dvs_w_max, dvs_h_max;
4393
4394                         dvs_w = vp_cfg->bayer_ds_out_res.width -
4395                                 vp_cfg->output_info[0].res.width;
4396                         dvs_h = vp_cfg->bayer_ds_out_res.height -
4397                                 vp_cfg->output_info[0].res.height;
4398                         dvs_w_max = rounddown(
4399                                         vp_cfg->output_info[0].res.width / 5,
4400                                         ATOM_ISP_STEP_WIDTH);
4401                         dvs_h_max = rounddown(
4402                                         vp_cfg->output_info[0].res.height / 5,
4403                                         ATOM_ISP_STEP_HEIGHT);
4404
4405                         config->dvs_envelop.width = min(dvs_w, dvs_w_max);
4406                         config->dvs_envelop.height = min(dvs_h, dvs_h_max);
4407                 }
4408
4409                 return 0;
4410         }
4411
4412         memcpy(&asd->params.css_param.wb_config, &config->wb_config,
4413                sizeof(struct atomisp_css_wb_config));
4414         memcpy(&asd->params.css_param.ob_config, &config->ob_config,
4415                sizeof(struct atomisp_css_ob_config));
4416         memcpy(&asd->params.css_param.dp_config, &config->dp_config,
4417                sizeof(struct atomisp_css_dp_config));
4418         memcpy(&asd->params.css_param.de_config, &config->de_config,
4419                sizeof(struct atomisp_css_de_config));
4420         memcpy(&asd->params.css_param.dz_config, &config->dz_config,
4421                sizeof(struct atomisp_css_dz_config));
4422         memcpy(&asd->params.css_param.ce_config, &config->ce_config,
4423                sizeof(struct atomisp_css_ce_config));
4424         memcpy(&asd->params.css_param.nr_config, &config->nr_config,
4425                sizeof(struct atomisp_css_nr_config));
4426         memcpy(&asd->params.css_param.ee_config, &config->ee_config,
4427                sizeof(struct atomisp_css_ee_config));
4428         memcpy(&asd->params.css_param.tnr_config, &config->tnr_config,
4429                sizeof(struct atomisp_css_tnr_config));
4430
4431         if (asd->params.color_effect == V4L2_COLORFX_NEGATIVE) {
4432                 asd->params.css_param.cc_config.matrix[3] = -config->cc_config.matrix[3];
4433                 asd->params.css_param.cc_config.matrix[4] = -config->cc_config.matrix[4];
4434                 asd->params.css_param.cc_config.matrix[5] = -config->cc_config.matrix[5];
4435                 asd->params.css_param.cc_config.matrix[6] = -config->cc_config.matrix[6];
4436                 asd->params.css_param.cc_config.matrix[7] = -config->cc_config.matrix[7];
4437                 asd->params.css_param.cc_config.matrix[8] = -config->cc_config.matrix[8];
4438         }
4439
4440         if (asd->params.color_effect != V4L2_COLORFX_SEPIA &&
4441             asd->params.color_effect != V4L2_COLORFX_BW) {
4442                 memcpy(&asd->params.css_param.cc_config, &config->cc_config,
4443                        sizeof(struct atomisp_css_cc_config));
4444                 atomisp_css_set_cc_config(asd, &asd->params.css_param.cc_config);
4445         }
4446
4447         atomisp_css_set_wb_config(asd, &asd->params.css_param.wb_config);
4448         atomisp_css_set_ob_config(asd, &asd->params.css_param.ob_config);
4449         atomisp_css_set_de_config(asd, &asd->params.css_param.de_config);
4450         atomisp_css_set_dz_config(asd, &asd->params.css_param.dz_config);
4451         atomisp_css_set_ce_config(asd, &asd->params.css_param.ce_config);
4452         atomisp_css_set_dp_config(asd, &asd->params.css_param.dp_config);
4453         atomisp_css_set_nr_config(asd, &asd->params.css_param.nr_config);
4454         atomisp_css_set_ee_config(asd, &asd->params.css_param.ee_config);
4455         atomisp_css_set_tnr_config(asd, &asd->params.css_param.tnr_config);
4456         asd->params.css_update_params_needed = true;
4457
4458         return 0;
4459 }
4460
4461 /*
4462  * Function to configure color effect of the image
4463  */
4464 int atomisp_color_effect(struct atomisp_sub_device *asd, int flag,
4465                          __s32 *effect)
4466 {
4467         struct atomisp_css_cc_config *cc_config = NULL;
4468         struct atomisp_css_macc_table *macc_table = NULL;
4469         struct atomisp_css_ctc_table *ctc_table = NULL;
4470         int ret = 0;
4471         struct v4l2_control control;
4472         struct atomisp_device *isp = asd->isp;
4473
4474         if (flag == 0) {
4475                 *effect = asd->params.color_effect;
4476                 return 0;
4477         }
4478
4479
4480         control.id = V4L2_CID_COLORFX;
4481         control.value = *effect;
4482         ret =
4483             v4l2_s_ctrl(NULL, isp->inputs[asd->input_curr].camera->ctrl_handler,
4484                         &control);
4485         /*
4486          * if set color effect to sensor successfully, return
4487          * 0 directly.
4488          */
4489         if (!ret) {
4490                 asd->params.color_effect = (u32)*effect;
4491                 return 0;
4492         }
4493
4494         if (*effect == asd->params.color_effect)
4495                 return 0;
4496
4497         /*
4498          * isp_subdev->params.macc_en should be set to false.
4499          */
4500         asd->params.macc_en = false;
4501
4502         switch (*effect) {
4503         case V4L2_COLORFX_NONE:
4504                 macc_table = &asd->params.css_param.macc_table;
4505                 asd->params.macc_en = true;
4506                 break;
4507         case V4L2_COLORFX_SEPIA:
4508                 cc_config = &sepia_cc_config;
4509                 break;
4510         case V4L2_COLORFX_NEGATIVE:
4511                 cc_config = &nega_cc_config;
4512                 break;
4513         case V4L2_COLORFX_BW:
4514                 cc_config = &mono_cc_config;
4515                 break;
4516         case V4L2_COLORFX_SKY_BLUE:
4517                 macc_table = &blue_macc_table;
4518                 asd->params.macc_en = true;
4519                 break;
4520         case V4L2_COLORFX_GRASS_GREEN:
4521                 macc_table = &green_macc_table;
4522                 asd->params.macc_en = true;
4523                 break;
4524         case V4L2_COLORFX_SKIN_WHITEN_LOW:
4525                 macc_table = &skin_low_macc_table;
4526                 asd->params.macc_en = true;
4527                 break;
4528         case V4L2_COLORFX_SKIN_WHITEN:
4529                 macc_table = &skin_medium_macc_table;
4530                 asd->params.macc_en = true;
4531                 break;
4532         case V4L2_COLORFX_SKIN_WHITEN_HIGH:
4533                 macc_table = &skin_high_macc_table;
4534                 asd->params.macc_en = true;
4535                 break;
4536         case V4L2_COLORFX_VIVID:
4537                 ctc_table = &vivid_ctc_table;
4538                 break;
4539         default:
4540                 return -EINVAL;
4541         }
4542         atomisp_update_capture_mode(asd);
4543
4544         if (cc_config)
4545                 atomisp_css_set_cc_config(asd, cc_config);
4546         if (macc_table)
4547                 atomisp_css_set_macc_table(asd, macc_table);
4548         if (ctc_table)
4549                 atomisp_css_set_ctc_table(asd, ctc_table);
4550         asd->params.color_effect = (u32)*effect;
4551         asd->params.css_update_params_needed = true;
4552         return 0;
4553 }
4554
4555 /*
4556  * Function to configure bad pixel correction
4557  */
4558 int atomisp_bad_pixel(struct atomisp_sub_device *asd, int flag,
4559                       __s32 *value)
4560 {
4561
4562         if (flag == 0) {
4563                 *value = asd->params.bad_pixel_en;
4564                 return 0;
4565         }
4566         asd->params.bad_pixel_en = !!*value;
4567
4568         return 0;
4569 }
4570
4571 /*
4572  * Function to configure bad pixel correction params
4573  */
4574 int atomisp_bad_pixel_param(struct atomisp_sub_device *asd, int flag,
4575                             struct atomisp_dp_config *config)
4576 {
4577         if (flag == 0) {
4578                 /* Get bad pixel from current setup */
4579                 if (atomisp_css_get_dp_config(asd, config))
4580                         return -EINVAL;
4581         } else {
4582                 /* Set bad pixel to isp parameters */
4583                 memcpy(&asd->params.css_param.dp_config, config,
4584                         sizeof(asd->params.css_param.dp_config));
4585                 atomisp_css_set_dp_config(asd, &asd->params.css_param.dp_config);
4586                 asd->params.css_update_params_needed = true;
4587         }
4588
4589         return 0;
4590 }
4591
4592 /*
4593  * Function to enable/disable video image stablization
4594  */
4595 int atomisp_video_stable(struct atomisp_sub_device *asd, int flag,
4596                          __s32 *value)
4597 {
4598         if (flag == 0)
4599                 *value = asd->params.video_dis_en;
4600         else
4601                 asd->params.video_dis_en = !!*value;
4602
4603         return 0;
4604 }
4605
4606 /*
4607  * Function to configure fixed pattern noise
4608  */
4609 int atomisp_fixed_pattern(struct atomisp_sub_device *asd, int flag,
4610                           __s32 *value)
4611 {
4612
4613         if (flag == 0) {
4614                 *value = asd->params.fpn_en;
4615                 return 0;
4616         }
4617
4618         if (*value == 0) {
4619                 asd->params.fpn_en = 0;
4620                 return 0;
4621         }
4622
4623         /* Add function to get black from from sensor with shutter off */
4624         return 0;
4625 }
4626
4627 static unsigned int
4628 atomisp_bytesperline_to_padded_width(unsigned int bytesperline,
4629                                      enum atomisp_css_frame_format format)
4630 {
4631         switch (format) {
4632         case CSS_FRAME_FORMAT_UYVY:
4633         case CSS_FRAME_FORMAT_YUYV:
4634         case CSS_FRAME_FORMAT_RAW:
4635         case CSS_FRAME_FORMAT_RGB565:
4636                 return bytesperline/2;
4637         case CSS_FRAME_FORMAT_RGBA888:
4638                 return bytesperline/4;
4639         /* The following cases could be removed, but we leave them
4640            in to document the formats that are included. */
4641         case CSS_FRAME_FORMAT_NV11:
4642         case CSS_FRAME_FORMAT_NV12:
4643         case CSS_FRAME_FORMAT_NV16:
4644         case CSS_FRAME_FORMAT_NV21:
4645         case CSS_FRAME_FORMAT_NV61:
4646         case CSS_FRAME_FORMAT_YV12:
4647         case CSS_FRAME_FORMAT_YV16:
4648         case CSS_FRAME_FORMAT_YUV420:
4649         case CSS_FRAME_FORMAT_YUV420_16:
4650         case CSS_FRAME_FORMAT_YUV422:
4651         case CSS_FRAME_FORMAT_YUV422_16:
4652         case CSS_FRAME_FORMAT_YUV444:
4653         case CSS_FRAME_FORMAT_YUV_LINE:
4654         case CSS_FRAME_FORMAT_PLANAR_RGB888:
4655         case CSS_FRAME_FORMAT_QPLANE6:
4656         case CSS_FRAME_FORMAT_BINARY_8:
4657         default:
4658                 return bytesperline;
4659         }
4660 }
4661
4662 static int
4663 atomisp_v4l2_framebuffer_to_css_frame(const struct v4l2_framebuffer *arg,
4664                                          struct atomisp_css_frame **result)
4665 {
4666         struct atomisp_css_frame *res = NULL;
4667         unsigned int padded_width;
4668         enum atomisp_css_frame_format sh_format;
4669         char *tmp_buf = NULL;
4670         int ret = 0;
4671
4672         sh_format = v4l2_fmt_to_sh_fmt(arg->fmt.pixelformat);
4673         padded_width = atomisp_bytesperline_to_padded_width(
4674                                         arg->fmt.bytesperline, sh_format);
4675
4676         /* Note: the padded width on an atomisp_css_frame is in elements, not in
4677            bytes. The RAW frame we use here should always be a 16bit RAW
4678            frame. This is why we bytesperline/2 is equal to the padded with */
4679         if (atomisp_css_frame_allocate(&res, arg->fmt.width, arg->fmt.height,
4680                                   sh_format, padded_width, 0)) {
4681                 ret = -ENOMEM;
4682                 goto err;
4683         }
4684
4685         tmp_buf = vmalloc(arg->fmt.sizeimage);
4686         if (!tmp_buf) {
4687                 ret = -ENOMEM;
4688                 goto err;
4689         }
4690         if (copy_from_user(tmp_buf, (void __user __force *)arg->base,
4691                            arg->fmt.sizeimage)) {
4692                 ret = -EFAULT;
4693                 goto err;
4694         }
4695
4696         if (hmm_store(res->data, tmp_buf, arg->fmt.sizeimage)) {
4697                 ret = -EINVAL;
4698                 goto err;
4699         }
4700
4701 err:
4702         if (ret && res)
4703                 atomisp_css_frame_free(res);
4704         if (tmp_buf)
4705                 vfree(tmp_buf);
4706         if (ret == 0)
4707                 *result = res;
4708         return ret;
4709 }
4710
4711 /*
4712  * Function to configure fixed pattern noise table
4713  */
4714 int atomisp_fixed_pattern_table(struct atomisp_sub_device *asd,
4715                                 struct v4l2_framebuffer *arg)
4716 {
4717         struct atomisp_css_frame *raw_black_frame = NULL;
4718         int ret;
4719
4720         if (arg == NULL)
4721                 return -EINVAL;
4722
4723         ret = atomisp_v4l2_framebuffer_to_css_frame(arg, &raw_black_frame);
4724         if (ret)
4725                 return ret;
4726         if (atomisp_css_set_black_frame(asd, raw_black_frame))
4727                 ret = -ENOMEM;
4728
4729         atomisp_css_frame_free(raw_black_frame);
4730         return ret;
4731 }
4732
4733 /*
4734  * Function to configure false color correction
4735  */
4736 int atomisp_false_color(struct atomisp_sub_device *asd, int flag,
4737                         __s32 *value)
4738 {
4739         /* Get nr config from current setup */
4740         if (flag == 0) {
4741                 *value = asd->params.false_color;
4742                 return 0;
4743         }
4744
4745         /* Set nr config to isp parameters */
4746         if (*value) {
4747                 atomisp_css_set_default_de_config(asd);
4748         } else {
4749                 asd->params.css_param.de_config.pixelnoise = 0;
4750                 atomisp_css_set_de_config(asd, &asd->params.css_param.de_config);
4751         }
4752         asd->params.css_update_params_needed = true;
4753         asd->params.false_color = *value;
4754         return 0;
4755 }
4756
4757 /*
4758  * Function to configure bad pixel correction params
4759  */
4760 int atomisp_false_color_param(struct atomisp_sub_device *asd, int flag,
4761                               struct atomisp_de_config *config)
4762 {
4763         if (flag == 0) {
4764                 /* Get false color from current setup */
4765                 if (atomisp_css_get_de_config(asd, config))
4766                         return -EINVAL;
4767         } else {
4768                 /* Set false color to isp parameters */
4769                 memcpy(&asd->params.css_param.de_config, config,
4770                        sizeof(asd->params.css_param.de_config));
4771                 atomisp_css_set_de_config(asd, &asd->params.css_param.de_config);
4772                 asd->params.css_update_params_needed = true;
4773         }
4774
4775         return 0;
4776 }
4777
4778 /*
4779  * Function to configure white balance params
4780  */
4781 int atomisp_white_balance_param(struct atomisp_sub_device *asd, int flag,
4782         struct atomisp_wb_config *config)
4783 {
4784         if (flag == 0) {
4785                 /* Get white balance from current setup */
4786                 if (atomisp_css_get_wb_config(asd, config))
4787                         return -EINVAL;
4788         } else {
4789                 /* Set white balance to isp parameters */
4790                 memcpy(&asd->params.css_param.wb_config, config,
4791                        sizeof(asd->params.css_param.wb_config));
4792                 atomisp_css_set_wb_config(asd, &asd->params.css_param.wb_config);
4793                 asd->params.css_update_params_needed = true;
4794         }
4795
4796         return 0;
4797 }
4798
4799 int atomisp_3a_config_param(struct atomisp_sub_device *asd, int flag,
4800                             struct atomisp_3a_config *config)
4801 {
4802         struct atomisp_device *isp = asd->isp;
4803
4804         dev_dbg(isp->dev, ">%s %d\n", __func__, flag);
4805
4806         if (flag == 0) {
4807                 /* Get white balance from current setup */
4808                 if (atomisp_css_get_3a_config(asd, config))
4809                         return -EINVAL;
4810         } else {
4811                 /* Set white balance to isp parameters */
4812                 memcpy(&asd->params.css_param.s3a_config, config,
4813                        sizeof(asd->params.css_param.s3a_config));
4814                 atomisp_css_set_3a_config(asd, &asd->params.css_param.s3a_config);
4815                 asd->params.css_update_params_needed = true;
4816         }
4817
4818         dev_dbg(isp->dev, "<%s %d\n", __func__, flag);
4819         return 0;
4820 }
4821
4822 /*
4823  * Function to setup digital zoom
4824  */
4825 int atomisp_digital_zoom(struct atomisp_sub_device *asd, int flag,
4826                          __s32 *value)
4827 {
4828         u32 zoom;
4829         struct atomisp_device *isp = asd->isp;
4830
4831         unsigned int max_zoom = MRFLD_MAX_ZOOM_FACTOR;
4832
4833         if (flag == 0) {
4834                 atomisp_css_get_zoom_factor(asd, &zoom);
4835                 *value = max_zoom - zoom;
4836         } else {
4837                 if (*value < 0)
4838                         return -EINVAL;
4839
4840                 zoom = max_zoom - min_t(u32, max_zoom - 1, *value);
4841                 atomisp_css_set_zoom_factor(asd, zoom);
4842
4843                 dev_dbg(isp->dev, "%s, zoom: %d\n", __func__, zoom);
4844                 asd->params.css_update_params_needed = true;
4845         }
4846
4847         return 0;
4848 }
4849
4850 /*
4851  * Function to get sensor specific info for current resolution,
4852  * which will be used for auto exposure conversion.
4853  */
4854 int atomisp_get_sensor_mode_data(struct atomisp_sub_device *asd,
4855                                  struct atomisp_sensor_mode_data *config)
4856 {
4857         struct camera_mipi_info *mipi_info;
4858         struct atomisp_device *isp = asd->isp;
4859
4860         mipi_info = atomisp_to_sensor_mipi_info(
4861                 isp->inputs[asd->input_curr].camera);
4862         if (mipi_info == NULL)
4863                 return -EINVAL;
4864
4865         memcpy(config, &mipi_info->data, sizeof(*config));
4866         return 0;
4867 }
4868
4869 int atomisp_get_fmt(struct video_device *vdev, struct v4l2_format *f)
4870 {
4871         struct atomisp_video_pipe *pipe = atomisp_to_video_pipe(vdev);
4872
4873         f->fmt.pix = pipe->pix;
4874
4875         return 0;
4876 }
4877
4878 static void __atomisp_update_stream_env(struct atomisp_sub_device *asd,
4879         uint16_t stream_index, struct atomisp_input_stream_info *stream_info)
4880 {
4881         int i;
4882
4883 #if defined(ISP2401_NEW_INPUT_SYSTEM)
4884         /* assign virtual channel id return from sensor driver query */
4885         asd->stream_env[stream_index].ch_id = stream_info->ch_id;
4886 #endif
4887         asd->stream_env[stream_index].isys_configs = stream_info->isys_configs;
4888         for (i = 0; i < stream_info->isys_configs; i++) {
4889                 asd->stream_env[stream_index].isys_info[i].input_format =
4890                         stream_info->isys_info[i].input_format;
4891                 asd->stream_env[stream_index].isys_info[i].width =
4892                         stream_info->isys_info[i].width;
4893                 asd->stream_env[stream_index].isys_info[i].height =
4894                         stream_info->isys_info[i].height;
4895         }
4896 }
4897
4898 static void __atomisp_init_stream_info(uint16_t stream_index,
4899                 struct atomisp_input_stream_info *stream_info)
4900 {
4901         int i;
4902
4903         stream_info->enable = 1;
4904         stream_info->stream = stream_index;
4905         stream_info->ch_id = 0;
4906         stream_info->isys_configs = 0;
4907         for (i = 0; i < MAX_STREAMS_PER_CHANNEL; i++) {
4908                 stream_info->isys_info[i].input_format = 0;
4909                 stream_info->isys_info[i].width = 0;
4910                 stream_info->isys_info[i].height = 0;
4911         }
4912 }
4913
4914 /* This function looks up the closest available resolution. */
4915 int atomisp_try_fmt(struct video_device *vdev, struct v4l2_format *f,
4916                                                 bool *res_overflow)
4917 {
4918         struct atomisp_device *isp = video_get_drvdata(vdev);
4919         struct atomisp_sub_device *asd = atomisp_to_video_pipe(vdev)->asd;
4920         struct v4l2_subdev_pad_config pad_cfg;
4921         struct v4l2_subdev_format format = {
4922                 .which = V4L2_SUBDEV_FORMAT_TRY,
4923         };
4924
4925         struct v4l2_mbus_framefmt *snr_mbus_fmt = &format.format;
4926         const struct atomisp_format_bridge *fmt;
4927         struct atomisp_input_stream_info *stream_info =
4928             (struct atomisp_input_stream_info *)snr_mbus_fmt->reserved;
4929         uint16_t stream_index;
4930         int source_pad = atomisp_subdev_source_pad(vdev);
4931         int ret;
4932
4933         if (isp->inputs[asd->input_curr].camera == NULL)
4934                 return -EINVAL;
4935
4936         stream_index = atomisp_source_pad_to_stream_id(asd, source_pad);
4937         fmt = atomisp_get_format_bridge(f->fmt.pix.pixelformat);
4938         if (fmt == NULL) {
4939                 dev_err(isp->dev, "unsupported pixelformat!\n");
4940                 fmt = atomisp_output_fmts;
4941         }
4942
4943 #ifdef ISP2401
4944         if (f->fmt.pix.width <= 0 || f->fmt.pix.height <= 0)
4945                 return -EINVAL;
4946
4947 #endif
4948         snr_mbus_fmt->code = fmt->mbus_code;
4949         snr_mbus_fmt->width = f->fmt.pix.width;
4950         snr_mbus_fmt->height = f->fmt.pix.height;
4951
4952         __atomisp_init_stream_info(stream_index, stream_info);
4953
4954         dev_dbg(isp->dev, "try_mbus_fmt: asking for %ux%u\n",
4955                 snr_mbus_fmt->width, snr_mbus_fmt->height);
4956
4957         ret = v4l2_subdev_call(isp->inputs[asd->input_curr].camera,
4958                                pad, set_fmt, &pad_cfg, &format);
4959         if (ret)
4960                 return ret;
4961
4962         dev_dbg(isp->dev, "try_mbus_fmt: got %ux%u\n",
4963                 snr_mbus_fmt->width, snr_mbus_fmt->height);
4964
4965         fmt = atomisp_get_format_bridge_from_mbus(snr_mbus_fmt->code);
4966         if (fmt == NULL) {
4967                 dev_err(isp->dev, "unknown sensor format 0x%8.8x\n",
4968                         snr_mbus_fmt->code);
4969                 return -EINVAL;
4970         }
4971
4972         f->fmt.pix.pixelformat = fmt->pixelformat;
4973
4974         /*
4975          * If the format is jpeg or custom RAW, then the width and height will
4976          * not satisfy the normal atomisp requirements and no need to check
4977          * the below conditions. So just assign to what is being returned from
4978          * the sensor driver.
4979          */
4980         if (f->fmt.pix.pixelformat == V4L2_PIX_FMT_JPEG ||
4981             f->fmt.pix.pixelformat == V4L2_PIX_FMT_CUSTOM_M10MO_RAW) {
4982                 f->fmt.pix.width = snr_mbus_fmt->width;
4983                 f->fmt.pix.height = snr_mbus_fmt->height;
4984                 return 0;
4985         }
4986
4987         if (snr_mbus_fmt->width < f->fmt.pix.width
4988             && snr_mbus_fmt->height < f->fmt.pix.height) {
4989                 f->fmt.pix.width = snr_mbus_fmt->width;
4990                 f->fmt.pix.height = snr_mbus_fmt->height;
4991                 /* Set the flag when resolution requested is
4992                  * beyond the max value supported by sensor
4993                  */
4994                 if (res_overflow != NULL)
4995                         *res_overflow = true;
4996         }
4997
4998         /* app vs isp */
4999         f->fmt.pix.width = rounddown(
5000                 clamp_t(u32, f->fmt.pix.width, ATOM_ISP_MIN_WIDTH,
5001                         ATOM_ISP_MAX_WIDTH), ATOM_ISP_STEP_WIDTH);
5002         f->fmt.pix.height = rounddown(
5003                 clamp_t(u32, f->fmt.pix.height, ATOM_ISP_MIN_HEIGHT,
5004                         ATOM_ISP_MAX_HEIGHT), ATOM_ISP_STEP_HEIGHT);
5005
5006         return 0;
5007 }
5008
5009 static int
5010 atomisp_try_fmt_file(struct atomisp_device *isp, struct v4l2_format *f)
5011 {
5012         u32 width = f->fmt.pix.width;
5013         u32 height = f->fmt.pix.height;
5014         u32 pixelformat = f->fmt.pix.pixelformat;
5015         enum v4l2_field field = f->fmt.pix.field;
5016         u32 depth;
5017
5018         if (!atomisp_get_format_bridge(pixelformat)) {
5019                 dev_err(isp->dev, "Wrong output pixelformat\n");
5020                 return -EINVAL;
5021         }
5022
5023         depth = get_pixel_depth(pixelformat);
5024
5025         if (field == V4L2_FIELD_ANY)
5026                 field = V4L2_FIELD_NONE;
5027         else if (field != V4L2_FIELD_NONE) {
5028                 dev_err(isp->dev, "Wrong output field\n");
5029                 return -EINVAL;
5030         }
5031
5032         f->fmt.pix.field = field;
5033         f->fmt.pix.width = clamp_t(u32,
5034                                    rounddown(width, (u32)ATOM_ISP_STEP_WIDTH),
5035                                    ATOM_ISP_MIN_WIDTH, ATOM_ISP_MAX_WIDTH);
5036         f->fmt.pix.height = clamp_t(u32, rounddown(height,
5037                                     (u32)ATOM_ISP_STEP_HEIGHT),
5038                                     ATOM_ISP_MIN_HEIGHT, ATOM_ISP_MAX_HEIGHT);
5039         f->fmt.pix.bytesperline = (width * depth) >> 3;
5040
5041         return 0;
5042 }
5043
5044 mipi_port_ID_t __get_mipi_port(struct atomisp_device *isp,
5045                                 enum atomisp_camera_port port)
5046 {
5047         switch (port) {
5048         case ATOMISP_CAMERA_PORT_PRIMARY:
5049                 return MIPI_PORT0_ID;
5050         case ATOMISP_CAMERA_PORT_SECONDARY:
5051                 return MIPI_PORT1_ID;
5052         case ATOMISP_CAMERA_PORT_TERTIARY:
5053                 if (MIPI_PORT1_ID + 1 != N_MIPI_PORT_ID)
5054                         return MIPI_PORT1_ID + 1;
5055                 /* go through down for else case */
5056         default:
5057                 dev_err(isp->dev, "unsupported port: %d\n", port);
5058                 return MIPI_PORT0_ID;
5059         }
5060 }
5061
5062 static inline int atomisp_set_sensor_mipi_to_isp(
5063                                 struct atomisp_sub_device *asd,
5064                                 enum atomisp_input_stream_id stream_id,
5065                                 struct camera_mipi_info *mipi_info)
5066 {
5067         struct v4l2_control ctrl;
5068         struct atomisp_device *isp = asd->isp;
5069         const struct atomisp_in_fmt_conv *fc;
5070         int mipi_freq = 0;
5071         unsigned int input_format, bayer_order;
5072
5073         ctrl.id = V4L2_CID_LINK_FREQ;
5074         if (v4l2_g_ctrl
5075             (isp->inputs[asd->input_curr].camera->ctrl_handler, &ctrl) == 0)
5076                 mipi_freq = ctrl.value;
5077
5078         if (asd->stream_env[stream_id].isys_configs == 1) {
5079                 input_format =
5080                         asd->stream_env[stream_id].isys_info[0].input_format;
5081                 atomisp_css_isys_set_format(asd, stream_id,
5082                                 input_format, IA_CSS_STREAM_DEFAULT_ISYS_STREAM_IDX);
5083         } else if (asd->stream_env[stream_id].isys_configs == 2) {
5084                 atomisp_css_isys_two_stream_cfg_update_stream1(
5085                                 asd, stream_id,
5086                                 asd->stream_env[stream_id].isys_info[0].input_format,
5087                                 asd->stream_env[stream_id].isys_info[0].width,
5088                                 asd->stream_env[stream_id].isys_info[0].height);
5089
5090                 atomisp_css_isys_two_stream_cfg_update_stream2(
5091                                 asd, stream_id,
5092                                 asd->stream_env[stream_id].isys_info[1].input_format,
5093                                 asd->stream_env[stream_id].isys_info[1].width,
5094                                 asd->stream_env[stream_id].isys_info[1].height);
5095         }
5096
5097         /* Compatibility for sensors which provide no media bus code
5098          * in s_mbus_framefmt() nor support pad formats. */
5099         if (mipi_info->input_format != -1) {
5100                 bayer_order = mipi_info->raw_bayer_order;
5101
5102                 /* Input stream config is still needs configured */
5103                 /* TODO: Check if this is necessary */
5104                 fc = atomisp_find_in_fmt_conv_by_atomisp_in_fmt(
5105                                                 mipi_info->input_format);
5106                 if (!fc)
5107                         return -EINVAL;
5108                 input_format = fc->css_stream_fmt;
5109         } else {
5110                 struct v4l2_mbus_framefmt *sink;
5111                 sink = atomisp_subdev_get_ffmt(&asd->subdev, NULL,
5112                                                V4L2_SUBDEV_FORMAT_ACTIVE,
5113                                                ATOMISP_SUBDEV_PAD_SINK);
5114                 fc = atomisp_find_in_fmt_conv(sink->code);
5115                 if (!fc)
5116                         return -EINVAL;
5117                 input_format = fc->css_stream_fmt;
5118                 bayer_order = fc->bayer_order;
5119         }
5120
5121         atomisp_css_input_set_format(asd, stream_id, input_format);
5122         atomisp_css_input_set_bayer_order(asd, stream_id, bayer_order);
5123
5124         fc = atomisp_find_in_fmt_conv_by_atomisp_in_fmt(
5125                                         mipi_info->metadata_format);
5126         if (!fc)
5127                 return -EINVAL;
5128         input_format = fc->css_stream_fmt;
5129         atomisp_css_input_configure_port(asd,
5130                                 __get_mipi_port(asd->isp, mipi_info->port),
5131                                 mipi_info->num_lanes,
5132                                 0xffff4, mipi_freq,
5133                                 input_format,
5134                                 mipi_info->metadata_width,
5135                                 mipi_info->metadata_height);
5136         return 0;
5137 }
5138
5139 static int __enable_continuous_mode(struct atomisp_sub_device *asd,
5140                                     bool enable)
5141 {
5142         struct atomisp_device *isp = asd->isp;
5143
5144         dev_dbg(isp->dev,
5145                 "continuous mode %d, raw buffers %d, stop preview %d\n",
5146                 enable, asd->continuous_raw_buffer_size->val,
5147                 !asd->continuous_viewfinder->val);
5148 #ifndef ISP2401
5149         atomisp_css_capture_set_mode(asd, CSS_CAPTURE_MODE_PRIMARY);
5150 #else
5151         atomisp_update_capture_mode(asd);
5152 #endif
5153         /* in case of ANR, force capture pipe to offline mode */
5154         atomisp_css_capture_enable_online(asd, ATOMISP_INPUT_STREAM_GENERAL,
5155                         asd->params.low_light ? false : !enable);
5156         atomisp_css_preview_enable_online(asd, ATOMISP_INPUT_STREAM_GENERAL,
5157                         !enable);
5158         atomisp_css_enable_continuous(asd, enable);
5159         atomisp_css_enable_cvf(asd, asd->continuous_viewfinder->val);
5160
5161         if (atomisp_css_continuous_set_num_raw_frames(asd,
5162                         asd->continuous_raw_buffer_size->val)) {
5163                 dev_err(isp->dev, "css_continuous_set_num_raw_frames failed\n");
5164                 return -EINVAL;
5165         }
5166
5167         if (!enable) {
5168                 atomisp_css_enable_raw_binning(asd, false);
5169                 atomisp_css_input_set_two_pixels_per_clock(asd, false);
5170         }
5171
5172         if (isp->inputs[asd->input_curr].type != FILE_INPUT)
5173                 atomisp_css_input_set_mode(asd, CSS_INPUT_MODE_SENSOR);
5174
5175         return atomisp_update_run_mode(asd);
5176 }
5177
5178 int configure_pp_input_nop(struct atomisp_sub_device *asd,
5179                            unsigned int width, unsigned int height)
5180 {
5181         return 0;
5182 }
5183
5184 int configure_output_nop(struct atomisp_sub_device *asd,
5185                          unsigned int width, unsigned int height,
5186                          unsigned int min_width,
5187                          enum atomisp_css_frame_format sh_fmt)
5188 {
5189         return 0;
5190 }
5191
5192 int get_frame_info_nop(struct atomisp_sub_device *asd,
5193                        struct atomisp_css_frame_info *finfo)
5194 {
5195         return 0;
5196 }
5197
5198 /**
5199  * Resets CSS parameters that depend on input resolution.
5200  *
5201  * Update params like CSS RAW binning, 2ppc mode and pp_input
5202  * which depend on input size, but are not automatically
5203  * handled in CSS when the input resolution is changed.
5204  */
5205 static int css_input_resolution_changed(struct atomisp_sub_device *asd,
5206                 struct v4l2_mbus_framefmt *ffmt)
5207 {
5208         struct atomisp_metadata_buf *md_buf = NULL, *_md_buf;
5209         unsigned int i;
5210
5211         dev_dbg(asd->isp->dev, "css_input_resolution_changed to %ux%u\n",
5212                 ffmt->width, ffmt->height);
5213
5214 #if defined(ISP2401_NEW_INPUT_SYSTEM)
5215         atomisp_css_input_set_two_pixels_per_clock(asd, false);
5216 #else
5217         atomisp_css_input_set_two_pixels_per_clock(asd, true);
5218 #endif
5219         if (asd->continuous_mode->val) {
5220                 /* Note for all checks: ffmt includes pad_w+pad_h */
5221                 if (asd->run_mode->val == ATOMISP_RUN_MODE_VIDEO ||
5222                     (ffmt->width >= 2048 || ffmt->height >= 1536)) {
5223                         /*
5224                          * For preview pipe, enable only if resolution
5225                          * is >= 3M for ISP2400.
5226                          */
5227                         atomisp_css_enable_raw_binning(asd, true);
5228                 }
5229         }
5230         /*
5231          * If sensor input changed, which means metadata resolution changed
5232          * together. Release all metadata buffers here to let it re-allocated
5233          * next time in reqbufs.
5234          */
5235         for (i = 0; i < ATOMISP_METADATA_TYPE_NUM; i++) {
5236                 list_for_each_entry_safe(md_buf, _md_buf, &asd->metadata[i],
5237                                          list) {
5238                         atomisp_css_free_metadata_buffer(md_buf);
5239                         list_del(&md_buf->list);
5240                         kfree(md_buf);
5241                 }
5242         }
5243         return 0;
5244
5245         /*
5246          * TODO: atomisp_css_preview_configure_pp_input() not
5247          *       reset due to CSS bug tracked as PSI BZ 115124
5248          */
5249 }
5250
5251 static int atomisp_set_fmt_to_isp(struct video_device *vdev,
5252                         struct atomisp_css_frame_info *output_info,
5253                         struct atomisp_css_frame_info *raw_output_info,
5254                         struct v4l2_pix_format *pix,
5255                         unsigned int source_pad)
5256 {
5257         struct camera_mipi_info *mipi_info;
5258         struct atomisp_device *isp = video_get_drvdata(vdev);
5259         struct atomisp_sub_device *asd = atomisp_to_video_pipe(vdev)->asd;
5260         const struct atomisp_format_bridge *format;
5261         struct v4l2_rect *isp_sink_crop;
5262         enum atomisp_css_pipe_id pipe_id;
5263         struct v4l2_subdev_fh fh;
5264         int (*configure_output)(struct atomisp_sub_device *asd,
5265                                 unsigned int width, unsigned int height,
5266                                 unsigned int min_width,
5267                                 enum atomisp_css_frame_format sh_fmt) =
5268                                                         configure_output_nop;
5269         int (*get_frame_info)(struct atomisp_sub_device *asd,
5270                               struct atomisp_css_frame_info *finfo) =
5271                                                         get_frame_info_nop;
5272         int (*configure_pp_input)(struct atomisp_sub_device *asd,
5273                                   unsigned int width, unsigned int height) =
5274                                                         configure_pp_input_nop;
5275         uint16_t stream_index = atomisp_source_pad_to_stream_id(asd, source_pad);
5276         const struct atomisp_in_fmt_conv *fc;
5277         int ret;
5278
5279         v4l2_fh_init(&fh.vfh, vdev);
5280
5281         isp_sink_crop = atomisp_subdev_get_rect(
5282                 &asd->subdev, NULL, V4L2_SUBDEV_FORMAT_ACTIVE,
5283                 ATOMISP_SUBDEV_PAD_SINK, V4L2_SEL_TGT_CROP);
5284
5285         format = atomisp_get_format_bridge(pix->pixelformat);
5286         if (format == NULL)
5287                 return -EINVAL;
5288
5289         if (isp->inputs[asd->input_curr].type != TEST_PATTERN &&
5290                 isp->inputs[asd->input_curr].type != FILE_INPUT) {
5291                 mipi_info = atomisp_to_sensor_mipi_info(
5292                         isp->inputs[asd->input_curr].camera);
5293                 if (!mipi_info) {
5294                         dev_err(isp->dev, "mipi_info is NULL\n");
5295                         return -EINVAL;
5296                 }
5297                 if (atomisp_set_sensor_mipi_to_isp(asd, stream_index,
5298                                         mipi_info))
5299                         return -EINVAL;
5300                 fc = atomisp_find_in_fmt_conv_by_atomisp_in_fmt(
5301                                 mipi_info->input_format);
5302                 if (!fc)
5303                         fc = atomisp_find_in_fmt_conv(
5304                                         atomisp_subdev_get_ffmt(&asd->subdev,
5305                                         NULL, V4L2_SUBDEV_FORMAT_ACTIVE,
5306                                         ATOMISP_SUBDEV_PAD_SINK)->code);
5307                 if (!fc)
5308                         return -EINVAL;
5309                 if (format->sh_fmt == CSS_FRAME_FORMAT_RAW &&
5310                      raw_output_format_match_input(fc->css_stream_fmt,
5311                         pix->pixelformat))
5312                         return -EINVAL;
5313         }
5314
5315         /*
5316          * Configure viewfinder also when vfpp is disabled: the
5317          * CSS still requires viewfinder configuration.
5318          */
5319         if (asd->fmt_auto->val ||
5320             asd->vfpp->val != ATOMISP_VFPP_ENABLE) {
5321                 struct v4l2_rect vf_size = {0};
5322                 struct v4l2_mbus_framefmt vf_ffmt = {0};
5323
5324                 if (pix->width < 640 || pix->height < 480) {
5325                         vf_size.width = pix->width;
5326                         vf_size.height = pix->height;
5327                 } else {
5328                         vf_size.width = 640;
5329                         vf_size.height = 480;
5330                 }
5331
5332                 /* FIXME: proper format name for this one. See
5333                    atomisp_output_fmts[] in atomisp_v4l2.c */
5334                 vf_ffmt.code = V4L2_MBUS_FMT_CUSTOM_YUV420;
5335
5336                 atomisp_subdev_set_selection(&asd->subdev, fh.pad,
5337                                              V4L2_SUBDEV_FORMAT_ACTIVE,
5338                                              ATOMISP_SUBDEV_PAD_SOURCE_VF,
5339                                              V4L2_SEL_TGT_COMPOSE, 0, &vf_size);
5340                 atomisp_subdev_set_ffmt(&asd->subdev, fh.pad,
5341                                         V4L2_SUBDEV_FORMAT_ACTIVE,
5342                                         ATOMISP_SUBDEV_PAD_SOURCE_VF, &vf_ffmt);
5343                 asd->video_out_vf.sh_fmt = CSS_FRAME_FORMAT_NV12;
5344
5345                 if (asd->vfpp->val == ATOMISP_VFPP_DISABLE_SCALER) {
5346                         atomisp_css_video_configure_viewfinder(asd,
5347                                 vf_size.width, vf_size.height, 0,
5348                                 asd->video_out_vf.sh_fmt);
5349                 } else if (asd->run_mode->val == ATOMISP_RUN_MODE_VIDEO) {
5350                         if (source_pad == ATOMISP_SUBDEV_PAD_SOURCE_PREVIEW ||
5351                             source_pad == ATOMISP_SUBDEV_PAD_SOURCE_VIDEO)
5352                                 atomisp_css_video_configure_viewfinder(asd,
5353                                         vf_size.width, vf_size.height, 0,
5354                                         asd->video_out_vf.sh_fmt);
5355                         else
5356                                 atomisp_css_capture_configure_viewfinder(asd,
5357                                         vf_size.width, vf_size.height, 0,
5358                                         asd->video_out_vf.sh_fmt);
5359                 } else if (source_pad != ATOMISP_SUBDEV_PAD_SOURCE_PREVIEW ||
5360                          asd->vfpp->val == ATOMISP_VFPP_DISABLE_LOWLAT) {
5361                         atomisp_css_capture_configure_viewfinder(asd,
5362                                 vf_size.width, vf_size.height, 0,
5363                                 asd->video_out_vf.sh_fmt);
5364                 }
5365         }
5366
5367         if (asd->continuous_mode->val) {
5368                 ret = __enable_continuous_mode(asd, true);
5369                 if (ret)
5370                         return -EINVAL;
5371         }
5372
5373         atomisp_css_input_set_mode(asd, CSS_INPUT_MODE_SENSOR);
5374         atomisp_css_disable_vf_pp(asd,
5375                         asd->vfpp->val != ATOMISP_VFPP_ENABLE);
5376
5377         /* ISP2401 new input system need to use copy pipe */
5378         if (asd->copy_mode) {
5379                 pipe_id = CSS_PIPE_ID_COPY;
5380                 atomisp_css_capture_enable_online(asd, stream_index, false);
5381         } else if (asd->vfpp->val == ATOMISP_VFPP_DISABLE_SCALER) {
5382                 /* video same in continuouscapture and online modes */
5383                 configure_output = atomisp_css_video_configure_output;
5384                 get_frame_info = atomisp_css_video_get_output_frame_info;
5385                 pipe_id = CSS_PIPE_ID_VIDEO;
5386         } else if (asd->run_mode->val == ATOMISP_RUN_MODE_VIDEO) {
5387                 if (!asd->continuous_mode->val) {
5388                         configure_output = atomisp_css_video_configure_output;
5389                         get_frame_info =
5390                                 atomisp_css_video_get_output_frame_info;
5391                         pipe_id = CSS_PIPE_ID_VIDEO;
5392                 } else {
5393                         if (source_pad == ATOMISP_SUBDEV_PAD_SOURCE_PREVIEW ||
5394                             source_pad == ATOMISP_SUBDEV_PAD_SOURCE_VIDEO) {
5395                                 configure_output =
5396                                         atomisp_css_video_configure_output;
5397                                 get_frame_info =
5398                                         atomisp_css_video_get_output_frame_info;
5399                                 configure_pp_input =
5400                                         atomisp_css_video_configure_pp_input;
5401                                 pipe_id = CSS_PIPE_ID_VIDEO;
5402                         } else {
5403                                 configure_output =
5404                                         atomisp_css_capture_configure_output;
5405                                 get_frame_info =
5406                                         atomisp_css_capture_get_output_frame_info;
5407                                 configure_pp_input =
5408                                         atomisp_css_capture_configure_pp_input;
5409                                 pipe_id = CSS_PIPE_ID_CAPTURE;
5410
5411                                 atomisp_update_capture_mode(asd);
5412                                 atomisp_css_capture_enable_online(asd, stream_index, false);
5413                         }
5414                 }
5415         } else if (source_pad == ATOMISP_SUBDEV_PAD_SOURCE_PREVIEW) {
5416                 configure_output = atomisp_css_preview_configure_output;
5417                 get_frame_info = atomisp_css_preview_get_output_frame_info;
5418                 configure_pp_input = atomisp_css_preview_configure_pp_input;
5419                 pipe_id = CSS_PIPE_ID_PREVIEW;
5420         } else {
5421                 /* CSS doesn't support low light mode on SOC cameras, so disable
5422                  * it. FIXME: if this is done elsewhere, it gives corrupted
5423                  * colors into thumbnail image.
5424                  */
5425                 if (isp->inputs[asd->input_curr].type == SOC_CAMERA)
5426                         asd->params.low_light = false;
5427
5428                 if (format->sh_fmt == CSS_FRAME_FORMAT_RAW) {
5429                         atomisp_css_capture_set_mode(asd, CSS_CAPTURE_MODE_RAW);
5430                         atomisp_css_enable_dz(asd, false);
5431                 } else {
5432                         atomisp_update_capture_mode(asd);
5433                 }
5434
5435                 if (!asd->continuous_mode->val)
5436                         /* in case of ANR, force capture pipe to offline mode */
5437                         atomisp_css_capture_enable_online(asd, stream_index,
5438                                         asd->params.low_light ?
5439                                         false : asd->params.online_process);
5440
5441                 configure_output = atomisp_css_capture_configure_output;
5442                 get_frame_info = atomisp_css_capture_get_output_frame_info;
5443                 configure_pp_input = atomisp_css_capture_configure_pp_input;
5444                 pipe_id = CSS_PIPE_ID_CAPTURE;
5445
5446                 if (!asd->params.online_process &&
5447                     !asd->continuous_mode->val) {
5448                         ret = atomisp_css_capture_get_output_raw_frame_info(asd,
5449                                                         raw_output_info);
5450                         if (ret)
5451                                 return ret;
5452                 }
5453                 if (!asd->continuous_mode->val && asd->run_mode->val
5454                     != ATOMISP_RUN_MODE_STILL_CAPTURE) {
5455                         dev_err(isp->dev,
5456                                     "Need to set the running mode first\n");
5457                         asd->run_mode->val = ATOMISP_RUN_MODE_STILL_CAPTURE;
5458                 }
5459         }
5460
5461         /*
5462          * to SOC camera, use yuvpp pipe.
5463          */
5464         if (ATOMISP_USE_YUVPP(asd))
5465                 pipe_id = CSS_PIPE_ID_YUVPP;
5466
5467         if (asd->copy_mode)
5468                 ret = atomisp_css_copy_configure_output(asd, stream_index,
5469                                 pix->width, pix->height,
5470                                 format->planar ? pix->bytesperline :
5471                                         pix->bytesperline * 8 / format->depth,
5472                                 format->sh_fmt);
5473         else
5474                 ret = configure_output(asd, pix->width, pix->height,
5475                                        format->planar ? pix->bytesperline :
5476                                        pix->bytesperline * 8 / format->depth,
5477                                        format->sh_fmt);
5478         if (ret) {
5479                 dev_err(isp->dev, "configure_output %ux%u, format %8.8x\n",
5480                         pix->width, pix->height, format->sh_fmt);
5481                 return -EINVAL;
5482         }
5483
5484         if (asd->continuous_mode->val &&
5485             (configure_pp_input == atomisp_css_preview_configure_pp_input ||
5486              configure_pp_input == atomisp_css_video_configure_pp_input)) {
5487                 /* for isp 2.2, configure pp input is available for continuous
5488                  * mode */
5489                 ret = configure_pp_input(asd, isp_sink_crop->width,
5490                                          isp_sink_crop->height);
5491                 if (ret) {
5492                         dev_err(isp->dev, "configure_pp_input %ux%u\n",
5493                                 isp_sink_crop->width,
5494                                 isp_sink_crop->height);
5495                         return -EINVAL;
5496                 }
5497         } else {
5498                 ret = configure_pp_input(asd, isp_sink_crop->width,
5499                                          isp_sink_crop->height);
5500                 if (ret) {
5501                         dev_err(isp->dev, "configure_pp_input %ux%u\n",
5502                                 isp_sink_crop->width, isp_sink_crop->height);
5503                         return -EINVAL;
5504                 }
5505         }
5506         if (asd->copy_mode)
5507                 ret = atomisp_css_copy_get_output_frame_info(asd, stream_index,
5508                                 output_info);
5509         else
5510                 ret = get_frame_info(asd, output_info);
5511         if (ret) {
5512                 dev_err(isp->dev, "get_frame_info %ux%u (padded to %u)\n",
5513                         pix->width, pix->height, pix->bytesperline);
5514                 return -EINVAL;
5515         }
5516
5517         atomisp_update_grid_info(asd, pipe_id, source_pad);
5518
5519         /* Free the raw_dump buffer first */
5520         atomisp_css_frame_free(asd->raw_output_frame);
5521         asd->raw_output_frame = NULL;
5522
5523         if (!asd->continuous_mode->val &&
5524             !asd->params.online_process && !isp->sw_contex.file_input &&
5525                 atomisp_css_frame_allocate_from_info(&asd->raw_output_frame,
5526                                                         raw_output_info))
5527                 return -ENOMEM;
5528
5529         return 0;
5530 }
5531
5532 static void atomisp_get_dis_envelop(struct atomisp_sub_device *asd,
5533                             unsigned int width, unsigned int height,
5534                             unsigned int *dvs_env_w, unsigned int *dvs_env_h)
5535 {
5536         struct atomisp_device *isp = asd->isp;
5537
5538         /* if subdev type is SOC camera,we do not need to set DVS */
5539         if (isp->inputs[asd->input_curr].type == SOC_CAMERA)
5540                 asd->params.video_dis_en = 0;
5541
5542         if (asd->params.video_dis_en &&
5543             asd->run_mode->val == ATOMISP_RUN_MODE_VIDEO) {
5544                 /* envelope is 20% of the output resolution */
5545                 /*
5546                  * dvs envelope cannot be round up.
5547                  * it would cause ISP timeout and color switch issue
5548                  */
5549                 *dvs_env_w = rounddown(width / 5, ATOM_ISP_STEP_WIDTH);
5550                 *dvs_env_h = rounddown(height / 5, ATOM_ISP_STEP_HEIGHT);
5551         }
5552
5553         asd->params.dis_proj_data_valid = false;
5554         asd->params.css_update_params_needed = true;
5555 }
5556
5557 static void atomisp_check_copy_mode(struct atomisp_sub_device *asd,
5558                 int source_pad, struct v4l2_format *f)
5559 {
5560 #if defined(ISP2401_NEW_INPUT_SYSTEM)
5561         struct v4l2_mbus_framefmt *sink, *src;
5562
5563         sink = atomisp_subdev_get_ffmt(&asd->subdev, NULL,
5564                 V4L2_SUBDEV_FORMAT_ACTIVE, ATOMISP_SUBDEV_PAD_SINK);
5565         src = atomisp_subdev_get_ffmt(&asd->subdev, NULL,
5566                 V4L2_SUBDEV_FORMAT_ACTIVE, source_pad);
5567
5568         if ((sink->code == src->code &&
5569             sink->width == f->fmt.pix.width &&
5570             sink->height == f->fmt.pix.height) ||
5571             ((asd->isp->inputs[asd->input_curr].type == SOC_CAMERA) &&
5572             (asd->isp->inputs[asd->input_curr].camera_caps->
5573             sensor[asd->sensor_curr].stream_num > 1)))
5574                 asd->copy_mode = true;
5575         else
5576 #endif
5577                 /* Only used for the new input system */
5578                 asd->copy_mode = false;
5579
5580         dev_dbg(asd->isp->dev, "copy_mode: %d\n", asd->copy_mode);
5581
5582 }
5583
5584 static int atomisp_set_fmt_to_snr(struct video_device *vdev,
5585                 struct v4l2_format *f, unsigned int pixelformat,
5586                 unsigned int padding_w, unsigned int padding_h,
5587                 unsigned int dvs_env_w, unsigned int dvs_env_h)
5588 {
5589         struct atomisp_sub_device *asd = atomisp_to_video_pipe(vdev)->asd;
5590         const struct atomisp_format_bridge *format;
5591         struct v4l2_subdev_pad_config pad_cfg;
5592         struct v4l2_subdev_format vformat = {
5593                 .which = V4L2_SUBDEV_FORMAT_TRY,
5594         };
5595         struct v4l2_mbus_framefmt *ffmt = &vformat.format;
5596         struct v4l2_mbus_framefmt *req_ffmt;
5597         struct atomisp_device *isp = asd->isp;
5598         struct atomisp_input_stream_info *stream_info =
5599             (struct atomisp_input_stream_info *)ffmt->reserved;
5600         uint16_t stream_index = ATOMISP_INPUT_STREAM_GENERAL;
5601         int source_pad = atomisp_subdev_source_pad(vdev);
5602         struct v4l2_subdev_fh fh;
5603         int ret;
5604
5605         v4l2_fh_init(&fh.vfh, vdev);
5606
5607         stream_index = atomisp_source_pad_to_stream_id(asd, source_pad);
5608
5609         format = atomisp_get_format_bridge(pixelformat);
5610         if (format == NULL)
5611                 return -EINVAL;
5612
5613         v4l2_fill_mbus_format(ffmt, &f->fmt.pix, format->mbus_code);
5614         ffmt->height += padding_h + dvs_env_h;
5615         ffmt->width += padding_w + dvs_env_w;
5616
5617         dev_dbg(isp->dev, "s_mbus_fmt: ask %ux%u (padding %ux%u, dvs %ux%u)\n",
5618                 ffmt->width, ffmt->height, padding_w, padding_h,
5619                 dvs_env_w, dvs_env_h);
5620
5621         __atomisp_init_stream_info(stream_index, stream_info);
5622
5623         req_ffmt = ffmt;
5624
5625         /* Disable dvs if resolution can't be supported by sensor */
5626         if (asd->params.video_dis_en &&
5627             source_pad == ATOMISP_SUBDEV_PAD_SOURCE_VIDEO) {
5628                 vformat.which = V4L2_SUBDEV_FORMAT_TRY;
5629                 ret = v4l2_subdev_call(isp->inputs[asd->input_curr].camera,
5630                         pad, set_fmt, &pad_cfg, &vformat);
5631                 if (ret)
5632                         return ret;
5633                 if (ffmt->width < req_ffmt->width ||
5634                     ffmt->height < req_ffmt->height) {
5635                         req_ffmt->height -= dvs_env_h;
5636                         req_ffmt->width -= dvs_env_w;
5637                         ffmt = req_ffmt;
5638                         dev_warn(isp->dev,
5639                           "can not enable video dis due to sensor limitation.");
5640                         asd->params.video_dis_en = 0;
5641                 }
5642         }
5643         dev_dbg(isp->dev, "sensor width: %d, height: %d\n",
5644                 ffmt->width, ffmt->height);
5645         vformat.which = V4L2_SUBDEV_FORMAT_ACTIVE;
5646         ret = v4l2_subdev_call(isp->inputs[asd->input_curr].camera, pad,
5647                                set_fmt, NULL, &vformat);
5648         if (ret)
5649                 return ret;
5650
5651         __atomisp_update_stream_env(asd, stream_index, stream_info);
5652
5653         dev_dbg(isp->dev, "sensor width: %d, height: %d\n",
5654                 ffmt->width, ffmt->height);
5655
5656         if (ffmt->width < ATOM_ISP_STEP_WIDTH ||
5657             ffmt->height < ATOM_ISP_STEP_HEIGHT)
5658                         return -EINVAL;
5659
5660         if (asd->params.video_dis_en &&
5661             source_pad == ATOMISP_SUBDEV_PAD_SOURCE_VIDEO &&
5662             (ffmt->width < req_ffmt->width || ffmt->height < req_ffmt->height)) {
5663                 dev_warn(isp->dev,
5664                          "can not enable video dis due to sensor limitation.");
5665                 asd->params.video_dis_en = 0;
5666         }
5667
5668         atomisp_subdev_set_ffmt(&asd->subdev, fh.pad,
5669                                 V4L2_SUBDEV_FORMAT_ACTIVE,
5670                                 ATOMISP_SUBDEV_PAD_SINK, ffmt);
5671
5672         return css_input_resolution_changed(asd, ffmt);
5673 }
5674
5675 int atomisp_set_fmt(struct video_device *vdev, struct v4l2_format *f)
5676 {
5677         struct atomisp_device *isp = video_get_drvdata(vdev);
5678         struct atomisp_video_pipe *pipe = atomisp_to_video_pipe(vdev);
5679         struct atomisp_sub_device *asd = pipe->asd;
5680         const struct atomisp_format_bridge *format_bridge;
5681         const struct atomisp_format_bridge *snr_format_bridge;
5682         struct atomisp_css_frame_info output_info, raw_output_info;
5683         struct v4l2_format snr_fmt = *f;
5684         struct v4l2_format backup_fmt = *f, s_fmt = *f;
5685         unsigned int dvs_env_w = 0, dvs_env_h = 0;
5686         unsigned int padding_w = pad_w, padding_h = pad_h;
5687         bool res_overflow = false, crop_needs_override = false;
5688         struct v4l2_mbus_framefmt isp_sink_fmt;
5689         struct v4l2_mbus_framefmt isp_source_fmt = {0};
5690         struct v4l2_rect isp_sink_crop;
5691         uint16_t source_pad = atomisp_subdev_source_pad(vdev);
5692         struct v4l2_subdev_fh fh;
5693         int ret;
5694
5695         dev_dbg(isp->dev,
5696                 "setting resolution %ux%u on pad %u for asd%d, bytesperline %u\n",
5697                 f->fmt.pix.width, f->fmt.pix.height, source_pad,
5698                 asd->index, f->fmt.pix.bytesperline);
5699
5700         if (source_pad >= ATOMISP_SUBDEV_PADS_NUM)
5701                 return -EINVAL;
5702
5703         if (asd->streaming == ATOMISP_DEVICE_STREAMING_ENABLED) {
5704                 dev_warn(isp->dev, "ISP does not support set format while at streaming!\n");
5705                 return -EBUSY;
5706         }
5707
5708         v4l2_fh_init(&fh.vfh, vdev);
5709
5710         format_bridge = atomisp_get_format_bridge(f->fmt.pix.pixelformat);
5711         if (format_bridge == NULL)
5712                 return -EINVAL;
5713
5714         pipe->sh_fmt = format_bridge->sh_fmt;
5715         pipe->pix.pixelformat = f->fmt.pix.pixelformat;
5716
5717         if (source_pad == ATOMISP_SUBDEV_PAD_SOURCE_VF ||
5718             (source_pad == ATOMISP_SUBDEV_PAD_SOURCE_PREVIEW
5719                 && asd->run_mode->val == ATOMISP_RUN_MODE_VIDEO)) {
5720                 if (asd->fmt_auto->val) {
5721                         struct v4l2_rect *capture_comp;
5722                         struct v4l2_rect r = {0};
5723
5724                         r.width = f->fmt.pix.width;
5725                         r.height = f->fmt.pix.height;
5726
5727                         if (source_pad == ATOMISP_SUBDEV_PAD_SOURCE_PREVIEW)
5728                                 capture_comp = atomisp_subdev_get_rect(
5729                                         &asd->subdev, NULL,
5730                                         V4L2_SUBDEV_FORMAT_ACTIVE,
5731                                         ATOMISP_SUBDEV_PAD_SOURCE_VIDEO,
5732                                         V4L2_SEL_TGT_COMPOSE);
5733                         else
5734                                 capture_comp = atomisp_subdev_get_rect(
5735                                         &asd->subdev, NULL,
5736                                         V4L2_SUBDEV_FORMAT_ACTIVE,
5737                                         ATOMISP_SUBDEV_PAD_SOURCE_CAPTURE,
5738                                         V4L2_SEL_TGT_COMPOSE);
5739
5740                         if (capture_comp->width < r.width
5741                             || capture_comp->height < r.height) {
5742                                 r.width = capture_comp->width;
5743                                 r.height = capture_comp->height;
5744                         }
5745
5746                         atomisp_subdev_set_selection(
5747                                 &asd->subdev, fh.pad,
5748                                 V4L2_SUBDEV_FORMAT_ACTIVE, source_pad,
5749                                 V4L2_SEL_TGT_COMPOSE, 0, &r);
5750
5751                         f->fmt.pix.width = r.width;
5752                         f->fmt.pix.height = r.height;
5753                 }
5754
5755                 if (source_pad == ATOMISP_SUBDEV_PAD_SOURCE_PREVIEW &&
5756                     (asd->isp->inputs[asd->input_curr].type == SOC_CAMERA) &&
5757                     (asd->isp->inputs[asd->input_curr].camera_caps->
5758                     sensor[asd->sensor_curr].stream_num > 1)) {
5759                         /* For M10MO outputing YUV preview images. */
5760                         uint16_t video_index =
5761                                 atomisp_source_pad_to_stream_id(asd,
5762                                         ATOMISP_SUBDEV_PAD_SOURCE_VIDEO);
5763
5764                         ret = atomisp_css_copy_get_output_frame_info(asd,
5765                                 video_index, &output_info);
5766                         if (ret) {
5767                                 dev_err(isp->dev,
5768                                       "copy_get_output_frame_info ret %i", ret);
5769                                 return -EINVAL;
5770                         }
5771                         if (!asd->yuvpp_mode) {
5772                                 /*
5773                                  * If viewfinder was configured into copy_mode,
5774                                  * we switch to using yuvpp pipe instead.
5775                                  */
5776                                 asd->yuvpp_mode = true;
5777                                 ret = atomisp_css_copy_configure_output(
5778                                         asd, video_index, 0, 0, 0, 0);
5779                                 if (ret) {
5780                                         dev_err(isp->dev,
5781                                                 "failed to disable copy pipe");
5782                                         return -EINVAL;
5783                                 }
5784                                 ret = atomisp_css_yuvpp_configure_output(
5785                                         asd, video_index,
5786                                         output_info.res.width,
5787                                         output_info.res.height,
5788                                         output_info.padded_width,
5789                                         output_info.format);
5790                                 if (ret) {
5791                                         dev_err(isp->dev,
5792                                                 "failed to set up yuvpp pipe\n");
5793                                         return -EINVAL;
5794                                 }
5795                                 atomisp_css_video_enable_online(asd, false);
5796                                 atomisp_css_preview_enable_online(asd,
5797                                         ATOMISP_INPUT_STREAM_GENERAL, false);
5798                         }
5799                         atomisp_css_yuvpp_configure_viewfinder(asd, video_index,
5800                                 f->fmt.pix.width, f->fmt.pix.height,
5801                                 format_bridge->planar ? f->fmt.pix.bytesperline
5802                                 : f->fmt.pix.bytesperline * 8
5803                                 / format_bridge->depth, format_bridge->sh_fmt);
5804                         atomisp_css_yuvpp_get_viewfinder_frame_info(
5805                                 asd, video_index, &output_info);
5806                 } else if (source_pad == ATOMISP_SUBDEV_PAD_SOURCE_PREVIEW) {
5807                         atomisp_css_video_configure_viewfinder(asd,
5808                                 f->fmt.pix.width, f->fmt.pix.height,
5809                                 format_bridge->planar ? f->fmt.pix.bytesperline
5810                                 : f->fmt.pix.bytesperline * 8
5811                                 / format_bridge->depth, format_bridge->sh_fmt);
5812                         atomisp_css_video_get_viewfinder_frame_info(asd,
5813                                                                 &output_info);
5814                         asd->copy_mode = false;
5815                 } else {
5816                         atomisp_css_capture_configure_viewfinder(asd,
5817                                 f->fmt.pix.width, f->fmt.pix.height,
5818                                 format_bridge->planar ? f->fmt.pix.bytesperline
5819                                 : f->fmt.pix.bytesperline * 8
5820                                 / format_bridge->depth, format_bridge->sh_fmt);
5821                         atomisp_css_capture_get_viewfinder_frame_info(asd,
5822                                                                 &output_info);
5823                         asd->copy_mode = false;
5824                 }
5825
5826                 goto done;
5827         }
5828         /*
5829          * Check whether main resolution configured smaller
5830          * than snapshot resolution. If so, force main resolution
5831          * to be the same as snapshot resolution
5832          */
5833         if (source_pad == ATOMISP_SUBDEV_PAD_SOURCE_CAPTURE) {
5834                 struct v4l2_rect *r;
5835
5836                 r = atomisp_subdev_get_rect(
5837                         &asd->subdev, NULL,
5838                         V4L2_SUBDEV_FORMAT_ACTIVE,
5839                         ATOMISP_SUBDEV_PAD_SOURCE_VF, V4L2_SEL_TGT_COMPOSE);
5840
5841                 if (r->width && r->height
5842                     && (r->width > f->fmt.pix.width
5843                         || r->height > f->fmt.pix.height))
5844                         dev_warn(isp->dev,
5845                                  "Main Resolution config smaller then Vf Resolution. Force to be equal with Vf Resolution.");
5846         }
5847
5848         /* Pipeline configuration done through subdevs. Bail out now. */
5849         if (!asd->fmt_auto->val)
5850                 goto set_fmt_to_isp;
5851
5852         /* get sensor resolution and format */
5853         ret = atomisp_try_fmt(vdev, &snr_fmt, &res_overflow);
5854         if (ret)
5855                 return ret;
5856         f->fmt.pix.width = snr_fmt.fmt.pix.width;
5857         f->fmt.pix.height = snr_fmt.fmt.pix.height;
5858
5859         snr_format_bridge =
5860                 atomisp_get_format_bridge(snr_fmt.fmt.pix.pixelformat);
5861         if (!snr_format_bridge)
5862                 return -EINVAL;
5863
5864         atomisp_subdev_get_ffmt(&asd->subdev, NULL,
5865                                 V4L2_SUBDEV_FORMAT_ACTIVE,
5866                                 ATOMISP_SUBDEV_PAD_SINK)->code =
5867                 snr_format_bridge->mbus_code;
5868
5869         isp_sink_fmt = *atomisp_subdev_get_ffmt(&asd->subdev, NULL,
5870                                             V4L2_SUBDEV_FORMAT_ACTIVE,
5871                                             ATOMISP_SUBDEV_PAD_SINK);
5872
5873         isp_source_fmt.code = format_bridge->mbus_code;
5874         atomisp_subdev_set_ffmt(&asd->subdev, fh.pad,
5875                                 V4L2_SUBDEV_FORMAT_ACTIVE,
5876                                 source_pad, &isp_source_fmt);
5877
5878         if (!atomisp_subdev_format_conversion(asd, source_pad)) {
5879                 padding_w = 0;
5880                 padding_h = 0;
5881         } else if (IS_BYT) {
5882                 padding_w = 12;
5883                 padding_h = 12;
5884         }
5885
5886         /* construct resolution supported by isp */
5887         if (res_overflow && !asd->continuous_mode->val) {
5888                 f->fmt.pix.width = rounddown(
5889                         clamp_t(u32, f->fmt.pix.width - padding_w,
5890                                 ATOM_ISP_MIN_WIDTH,
5891                                 ATOM_ISP_MAX_WIDTH), ATOM_ISP_STEP_WIDTH);
5892                 f->fmt.pix.height = rounddown(
5893                         clamp_t(u32, f->fmt.pix.height - padding_h,
5894                                 ATOM_ISP_MIN_HEIGHT,
5895                                 ATOM_ISP_MAX_HEIGHT), ATOM_ISP_STEP_HEIGHT);
5896         }
5897
5898         atomisp_get_dis_envelop(asd, f->fmt.pix.width, f->fmt.pix.height,
5899                                 &dvs_env_w, &dvs_env_h);
5900
5901         if (asd->continuous_mode->val) {
5902                 struct v4l2_rect *r;
5903
5904                 r = atomisp_subdev_get_rect(
5905                         &asd->subdev, NULL,
5906                         V4L2_SUBDEV_FORMAT_ACTIVE,
5907                         ATOMISP_SUBDEV_PAD_SOURCE_CAPTURE,
5908                         V4L2_SEL_TGT_COMPOSE);
5909                 /*
5910                  * The ATOMISP_SUBDEV_PAD_SOURCE_CAPTURE should get resolutions
5911                  * properly set otherwise, it should not be the capture_pad.
5912                  */
5913                 if (r->width && r->height)
5914                         asd->capture_pad = ATOMISP_SUBDEV_PAD_SOURCE_CAPTURE;
5915                 else
5916                         asd->capture_pad = source_pad;
5917         } else {
5918                 asd->capture_pad = source_pad;
5919         }
5920         /*
5921          * set format info to sensor
5922          * In continuous mode, resolution is set only if it is higher than
5923          * existing value. This because preview pipe will be configured after
5924          * capture pipe and usually has lower resolution than capture pipe.
5925          */
5926         if (!asd->continuous_mode->val ||
5927             isp_sink_fmt.width < (f->fmt.pix.width + padding_w + dvs_env_w) ||
5928             isp_sink_fmt.height < (f->fmt.pix.height + padding_h +
5929                                     dvs_env_h)) {
5930                 /*
5931                  * For jpeg or custom raw format the sensor will return constant
5932                  * width and height. Because we already had quried try_mbus_fmt,
5933                  * f->fmt.pix.width and f->fmt.pix.height has been changed to
5934                  * this fixed width and height. So we cannot select the correct
5935                  * resolution with that information. So use the original width
5936                  * and height while set_mbus_fmt() so actual resolutions are
5937                  * being used in while set media bus format.
5938                  */
5939                 s_fmt = *f;
5940                 if (f->fmt.pix.pixelformat == V4L2_PIX_FMT_JPEG ||
5941                     f->fmt.pix.pixelformat == V4L2_PIX_FMT_CUSTOM_M10MO_RAW) {
5942                         s_fmt.fmt.pix.width = backup_fmt.fmt.pix.width;
5943                         s_fmt.fmt.pix.height = backup_fmt.fmt.pix.height;
5944                 }
5945                 ret = atomisp_set_fmt_to_snr(vdev, &s_fmt,
5946                                         f->fmt.pix.pixelformat, padding_w,
5947                                         padding_h, dvs_env_w, dvs_env_h);
5948                 if (ret)
5949                         return -EINVAL;
5950
5951                 atomisp_csi_lane_config(isp);
5952                 crop_needs_override = true;
5953         }
5954
5955         atomisp_check_copy_mode(asd, source_pad, &backup_fmt);
5956         asd->yuvpp_mode = false;                        /* Reset variable */
5957
5958         isp_sink_crop = *atomisp_subdev_get_rect(&asd->subdev, NULL,
5959                                                  V4L2_SUBDEV_FORMAT_ACTIVE,
5960                                                  ATOMISP_SUBDEV_PAD_SINK,
5961                                                  V4L2_SEL_TGT_CROP);
5962
5963         /* Try to enable YUV downscaling if ISP input is 10 % (either
5964          * width or height) bigger than the desired result. */
5965         if (isp_sink_crop.width * 9 / 10 < f->fmt.pix.width ||
5966             isp_sink_crop.height * 9 / 10 < f->fmt.pix.height ||
5967             (atomisp_subdev_format_conversion(asd, source_pad) &&
5968             ((asd->run_mode->val == ATOMISP_RUN_MODE_VIDEO &&
5969                !asd->continuous_mode->val) ||
5970               asd->vfpp->val == ATOMISP_VFPP_DISABLE_SCALER))) {
5971                 /* for continuous mode, preview size might be smaller than
5972                  * still capture size. if preview size still needs crop,
5973                  * pick the larger one between crop size of preview and
5974                  * still capture.
5975                  */
5976                 if (asd->continuous_mode->val
5977                     && source_pad == ATOMISP_SUBDEV_PAD_SOURCE_PREVIEW
5978                     && !crop_needs_override) {
5979                         isp_sink_crop.width =
5980                                 max_t(unsigned int, f->fmt.pix.width,
5981                                       isp_sink_crop.width);
5982                         isp_sink_crop.height =
5983                                 max_t(unsigned int, f->fmt.pix.height,
5984                                       isp_sink_crop.height);
5985                 } else {
5986                         isp_sink_crop.width = f->fmt.pix.width;
5987                         isp_sink_crop.height = f->fmt.pix.height;
5988                 }
5989
5990                 atomisp_subdev_set_selection(&asd->subdev, fh.pad,
5991                                              V4L2_SUBDEV_FORMAT_ACTIVE,
5992                                              ATOMISP_SUBDEV_PAD_SINK,
5993                                              V4L2_SEL_TGT_CROP,
5994                                              V4L2_SEL_FLAG_KEEP_CONFIG,
5995                                              &isp_sink_crop);
5996                 atomisp_subdev_set_selection(&asd->subdev, fh.pad,
5997                                              V4L2_SUBDEV_FORMAT_ACTIVE,
5998                                              source_pad, V4L2_SEL_TGT_COMPOSE,
5999                                              0, &isp_sink_crop);
6000         } else if (IS_MOFD) {
6001                 struct v4l2_rect main_compose = {0};
6002
6003                 main_compose.width = isp_sink_crop.width;
6004                 main_compose.height =
6005                         DIV_ROUND_UP(main_compose.width * f->fmt.pix.height,
6006                                      f->fmt.pix.width);
6007                 if (main_compose.height > isp_sink_crop.height) {
6008                         main_compose.height = isp_sink_crop.height;
6009                         main_compose.width =
6010                                 DIV_ROUND_UP(main_compose.height *
6011                                              f->fmt.pix.width,
6012                                              f->fmt.pix.height);
6013                 }
6014
6015                 atomisp_subdev_set_selection(&asd->subdev, fh.pad,
6016                                 V4L2_SUBDEV_FORMAT_ACTIVE,
6017                                 source_pad,
6018                                 V4L2_SEL_TGT_COMPOSE, 0,
6019                                 &main_compose);
6020         } else {
6021                 struct v4l2_rect sink_crop = {0};
6022                 struct v4l2_rect main_compose = {0};
6023
6024                 main_compose.width = f->fmt.pix.width;
6025                 main_compose.height = f->fmt.pix.height;
6026
6027 #ifndef ISP2401
6028                 /* WORKAROUND: this override is universally enabled in
6029                  * GMIN to work around a CTS failures (GMINL-539)
6030                  * which appears to be related by a hardware
6031                  * performance limitation.  It's unclear why this
6032                  * particular code triggers the issue. */
6033                 if (1 ||
6034                     crop_needs_override) {
6035 #else
6036                 if (crop_needs_override) {
6037 #endif
6038                         if (isp_sink_crop.width * main_compose.height >
6039                             isp_sink_crop.height * main_compose.width) {
6040                                 sink_crop.height = isp_sink_crop.height;
6041                                 sink_crop.width = DIV_NEAREST_STEP(
6042                                                 sink_crop.height *
6043                                                 f->fmt.pix.width,
6044                                                 f->fmt.pix.height,
6045                                                 ATOM_ISP_STEP_WIDTH);
6046                         } else {
6047                                 sink_crop.width = isp_sink_crop.width;
6048                                 sink_crop.height = DIV_NEAREST_STEP(
6049                                                 sink_crop.width *
6050                                                 f->fmt.pix.height,
6051                                                 f->fmt.pix.width,
6052                                                 ATOM_ISP_STEP_HEIGHT);
6053                         }
6054                         atomisp_subdev_set_selection(&asd->subdev, fh.pad,
6055                                 V4L2_SUBDEV_FORMAT_ACTIVE,
6056                                 ATOMISP_SUBDEV_PAD_SINK,
6057                                 V4L2_SEL_TGT_CROP,
6058                                 V4L2_SEL_FLAG_KEEP_CONFIG,
6059                                 &sink_crop);
6060                 }
6061                 atomisp_subdev_set_selection(&asd->subdev, fh.pad,
6062                                 V4L2_SUBDEV_FORMAT_ACTIVE,
6063                                 source_pad,
6064                                 V4L2_SEL_TGT_COMPOSE, 0,
6065                                 &main_compose);
6066         }
6067
6068 set_fmt_to_isp:
6069         ret = atomisp_set_fmt_to_isp(vdev, &output_info, &raw_output_info,
6070                                      &f->fmt.pix, source_pad);
6071         if (ret)
6072                 return -EINVAL;
6073 done:
6074         pipe->pix.width = f->fmt.pix.width;
6075         pipe->pix.height = f->fmt.pix.height;
6076         pipe->pix.pixelformat = f->fmt.pix.pixelformat;
6077         if (format_bridge->planar) {
6078                 pipe->pix.bytesperline = output_info.padded_width;
6079                 pipe->pix.sizeimage = PAGE_ALIGN(f->fmt.pix.height *
6080                         DIV_ROUND_UP(format_bridge->depth *
6081                                      output_info.padded_width, 8));
6082         } else {
6083                 pipe->pix.bytesperline =
6084                         DIV_ROUND_UP(format_bridge->depth *
6085                                      output_info.padded_width, 8);
6086                 pipe->pix.sizeimage =
6087                         PAGE_ALIGN(f->fmt.pix.height * pipe->pix.bytesperline);
6088
6089         }
6090         if (f->fmt.pix.field == V4L2_FIELD_ANY)
6091                 f->fmt.pix.field = V4L2_FIELD_NONE;
6092         pipe->pix.field = f->fmt.pix.field;
6093
6094         f->fmt.pix = pipe->pix;
6095         f->fmt.pix.priv = PAGE_ALIGN(pipe->pix.width *
6096                                      pipe->pix.height * 2);
6097
6098         pipe->capq.field = f->fmt.pix.field;
6099
6100         /*
6101          * If in video 480P case, no GFX throttle
6102          */
6103         if (asd->run_mode->val == ATOMISP_SUBDEV_PAD_SOURCE_VIDEO &&
6104             f->fmt.pix.width == 720 && f->fmt.pix.height == 480)
6105                 isp->need_gfx_throttle = false;
6106         else
6107                 isp->need_gfx_throttle = true;
6108
6109         return 0;
6110 }
6111
6112 int atomisp_set_fmt_file(struct video_device *vdev, struct v4l2_format *f)
6113 {
6114         struct atomisp_device *isp = video_get_drvdata(vdev);
6115         struct atomisp_video_pipe *pipe = atomisp_to_video_pipe(vdev);
6116         struct atomisp_sub_device *asd = pipe->asd;
6117         struct v4l2_mbus_framefmt ffmt = {0};
6118         const struct atomisp_format_bridge *format_bridge;
6119         struct v4l2_subdev_fh fh;
6120         int ret;
6121
6122         v4l2_fh_init(&fh.vfh, vdev);
6123
6124         dev_dbg(isp->dev, "setting fmt %ux%u 0x%x for file inject\n",
6125                 f->fmt.pix.width, f->fmt.pix.height, f->fmt.pix.pixelformat);
6126         ret = atomisp_try_fmt_file(isp, f);
6127         if (ret) {
6128                 dev_err(isp->dev, "atomisp_try_fmt_file err: %d\n", ret);
6129                 return ret;
6130         }
6131
6132         format_bridge = atomisp_get_format_bridge(f->fmt.pix.pixelformat);
6133         if (format_bridge == NULL) {
6134                 dev_dbg(isp->dev, "atomisp_get_format_bridge err! fmt:0x%x\n",
6135                                 f->fmt.pix.pixelformat);
6136                 return -EINVAL;
6137         }
6138
6139         pipe->pix = f->fmt.pix;
6140         atomisp_css_input_set_mode(asd, CSS_INPUT_MODE_FIFO);
6141         atomisp_css_input_configure_port(asd,
6142                 __get_mipi_port(isp, ATOMISP_CAMERA_PORT_PRIMARY), 2, 0xffff4,
6143                 0, 0, 0, 0);
6144         ffmt.width = f->fmt.pix.width;
6145         ffmt.height = f->fmt.pix.height;
6146         ffmt.code = format_bridge->mbus_code;
6147
6148         atomisp_subdev_set_ffmt(&asd->subdev, fh.pad, V4L2_SUBDEV_FORMAT_ACTIVE,
6149                                 ATOMISP_SUBDEV_PAD_SINK, &ffmt);
6150
6151         return 0;
6152 }
6153
6154 int atomisp_set_shading_table(struct atomisp_sub_device *asd,
6155                 struct atomisp_shading_table *user_shading_table)
6156 {
6157         struct atomisp_css_shading_table *shading_table;
6158         struct atomisp_css_shading_table *free_table;
6159         unsigned int len_table;
6160         int i;
6161         int ret = 0;
6162
6163         if (!user_shading_table)
6164                 return -EINVAL;
6165
6166         if (!user_shading_table->enable) {
6167                 atomisp_css_set_shading_table(asd, NULL);
6168                 asd->params.sc_en = 0;
6169                 return 0;
6170         }
6171
6172         /* If enabling, all tables must be set */
6173         for (i = 0; i < ATOMISP_NUM_SC_COLORS; i++) {
6174                 if (!user_shading_table->data[i])
6175                         return -EINVAL;
6176         }
6177
6178         /* Shading table size per color */
6179         if (user_shading_table->width > SH_CSS_MAX_SCTBL_WIDTH_PER_COLOR ||
6180             user_shading_table->height > SH_CSS_MAX_SCTBL_HEIGHT_PER_COLOR)
6181                 return -EINVAL;
6182
6183         shading_table = atomisp_css_shading_table_alloc(
6184                         user_shading_table->width, user_shading_table->height);
6185         if (!shading_table)
6186                 return -ENOMEM;
6187
6188         len_table = user_shading_table->width * user_shading_table->height *
6189                     ATOMISP_SC_TYPE_SIZE;
6190         for (i = 0; i < ATOMISP_NUM_SC_COLORS; i++) {
6191                 ret = copy_from_user(shading_table->data[i],
6192                                      user_shading_table->data[i], len_table);
6193                 if (ret) {
6194                         free_table = shading_table;
6195                         ret = -EFAULT;
6196                         goto out;
6197                 }
6198         }
6199         shading_table->sensor_width = user_shading_table->sensor_width;
6200         shading_table->sensor_height = user_shading_table->sensor_height;
6201         shading_table->fraction_bits = user_shading_table->fraction_bits;
6202
6203         free_table = asd->params.css_param.shading_table;
6204         asd->params.css_param.shading_table = shading_table;
6205         atomisp_css_set_shading_table(asd, shading_table);
6206         asd->params.sc_en = 1;
6207
6208 out:
6209         if (free_table != NULL)
6210                 atomisp_css_shading_table_free(free_table);
6211
6212         return ret;
6213 }
6214
6215 /*Turn off ISP dphy */
6216 int atomisp_ospm_dphy_down(struct atomisp_device *isp)
6217 {
6218         unsigned long flags;
6219         u32 reg;
6220
6221         dev_dbg(isp->dev, "%s\n", __func__);
6222
6223         /* if ISP timeout, we can force powerdown */
6224         if (isp->isp_timeout)
6225                 goto done;
6226
6227         if (!atomisp_dev_users(isp))
6228                 goto done;
6229
6230         spin_lock_irqsave(&isp->lock, flags);
6231         isp->sw_contex.power_state = ATOM_ISP_POWER_DOWN;
6232         spin_unlock_irqrestore(&isp->lock, flags);
6233 done:
6234         /*
6235          * MRFLD IUNIT DPHY is located in an always-power-on island
6236          * MRFLD HW design need all CSI ports are disabled before
6237          * powering down the IUNIT.
6238          */
6239         pci_read_config_dword(isp->pdev, MRFLD_PCI_CSI_CONTROL, &reg);
6240         reg |= MRFLD_ALL_CSI_PORTS_OFF_MASK;
6241         pci_write_config_dword(isp->pdev, MRFLD_PCI_CSI_CONTROL, reg);
6242         return 0;
6243 }
6244
6245 /*Turn on ISP dphy */
6246 int atomisp_ospm_dphy_up(struct atomisp_device *isp)
6247 {
6248         unsigned long flags;
6249         dev_dbg(isp->dev, "%s\n", __func__);
6250
6251         spin_lock_irqsave(&isp->lock, flags);
6252         isp->sw_contex.power_state = ATOM_ISP_POWER_UP;
6253         spin_unlock_irqrestore(&isp->lock, flags);
6254
6255         return 0;
6256 }
6257
6258
6259 int atomisp_exif_makernote(struct atomisp_sub_device *asd,
6260                            struct atomisp_makernote_info *config)
6261 {
6262         struct v4l2_control ctrl;
6263         struct atomisp_device *isp = asd->isp;
6264
6265         ctrl.id = V4L2_CID_FOCAL_ABSOLUTE;
6266         if (v4l2_g_ctrl
6267             (isp->inputs[asd->input_curr].camera->ctrl_handler, &ctrl)) {
6268                 dev_warn(isp->dev, "failed to g_ctrl for focal length\n");
6269                 return -EINVAL;
6270         } else {
6271                 config->focal_length = ctrl.value;
6272         }
6273
6274         ctrl.id = V4L2_CID_FNUMBER_ABSOLUTE;
6275         if (v4l2_g_ctrl
6276             (isp->inputs[asd->input_curr].camera->ctrl_handler, &ctrl)) {
6277                 dev_warn(isp->dev, "failed to g_ctrl for f-number\n");
6278                 return -EINVAL;
6279         } else {
6280                 config->f_number_curr = ctrl.value;
6281         }
6282
6283         ctrl.id = V4L2_CID_FNUMBER_RANGE;
6284         if (v4l2_g_ctrl
6285             (isp->inputs[asd->input_curr].camera->ctrl_handler, &ctrl)) {
6286                 dev_warn(isp->dev, "failed to g_ctrl for f number range\n");
6287                 return -EINVAL;
6288         } else {
6289                 config->f_number_range = ctrl.value;
6290         }
6291
6292         return 0;
6293 }
6294
6295 int atomisp_offline_capture_configure(struct atomisp_sub_device *asd,
6296                               struct atomisp_cont_capture_conf *cvf_config)
6297 {
6298         struct v4l2_ctrl *c;
6299
6300         /*
6301         * In case of M10MO ZSL capture case, we need to issue a separate
6302         * capture request to M10MO which will output captured jpeg image
6303         */
6304         c = v4l2_ctrl_find(
6305                 asd->isp->inputs[asd->input_curr].camera->ctrl_handler,
6306                 V4L2_CID_START_ZSL_CAPTURE);
6307         if (c) {
6308                 int ret;
6309                 dev_dbg(asd->isp->dev, "%s trigger ZSL capture request\n",
6310                         __func__);
6311                 /* TODO: use the cvf_config */
6312                 ret = v4l2_ctrl_s_ctrl(c, 1);
6313                 if (ret)
6314                         return ret;
6315
6316                 return v4l2_ctrl_s_ctrl(c, 0);
6317         }
6318
6319         asd->params.offline_parm = *cvf_config;
6320
6321         if (asd->params.offline_parm.num_captures) {
6322                 if (asd->streaming == ATOMISP_DEVICE_STREAMING_DISABLED) {
6323                         unsigned int init_raw_num;
6324
6325                         if (asd->enable_raw_buffer_lock->val) {
6326                                 init_raw_num =
6327                                         ATOMISP_CSS2_NUM_OFFLINE_INIT_CONTINUOUS_FRAMES_LOCK_EN;
6328                                 if (asd->run_mode->val == ATOMISP_RUN_MODE_VIDEO &&
6329                                     asd->params.video_dis_en)
6330                                         init_raw_num +=
6331                                             ATOMISP_CSS2_NUM_DVS_FRAME_DELAY;
6332                         } else {
6333                                 init_raw_num =
6334                                         ATOMISP_CSS2_NUM_OFFLINE_INIT_CONTINUOUS_FRAMES;
6335                         }
6336
6337                         /* TODO: this can be removed once user-space
6338                          *       has been updated to use control API */
6339                         asd->continuous_raw_buffer_size->val =
6340                                 max_t(int,
6341                                       asd->continuous_raw_buffer_size->val,
6342                                       asd->params.offline_parm.
6343                                       num_captures + init_raw_num);
6344                         asd->continuous_raw_buffer_size->val =
6345                                 min_t(int, ATOMISP_CONT_RAW_FRAMES,
6346                                       asd->continuous_raw_buffer_size->val);
6347                 }
6348                 asd->continuous_mode->val = true;
6349         } else {
6350                 asd->continuous_mode->val = false;
6351                 __enable_continuous_mode(asd, false);
6352         }
6353
6354         return 0;
6355 }
6356
6357 /*
6358  * set auto exposure metering window to camera sensor
6359  */
6360 int atomisp_s_ae_window(struct atomisp_sub_device *asd,
6361                         struct atomisp_ae_window *arg)
6362 {
6363         struct atomisp_device *isp = asd->isp;
6364         /* Coverity CID 298071 - initialzize struct */
6365         struct v4l2_subdev_selection sel = { 0 };
6366
6367         sel.r.left = arg->x_left;
6368         sel.r.top = arg->y_top;
6369         sel.r.width = arg->x_right - arg->x_left + 1;
6370         sel.r.height = arg->y_bottom - arg->y_top + 1;
6371
6372         if (v4l2_subdev_call(isp->inputs[asd->input_curr].camera,
6373                              pad, set_selection, NULL, &sel)) {
6374                 dev_err(isp->dev, "failed to call sensor set_selection.\n");
6375                 return -EINVAL;
6376         }
6377
6378         return 0;
6379 }
6380
6381 int atomisp_flash_enable(struct atomisp_sub_device *asd, int num_frames)
6382 {
6383         struct atomisp_device *isp = asd->isp;
6384
6385         if (num_frames < 0) {
6386                 dev_dbg(isp->dev, "%s ERROR: num_frames: %d\n", __func__,
6387                                 num_frames);
6388                 return -EINVAL;
6389         }
6390         /* a requested flash is still in progress. */
6391         if (num_frames && asd->params.flash_state != ATOMISP_FLASH_IDLE) {
6392                 dev_dbg(isp->dev, "%s flash busy: %d frames left: %d\n",
6393                                 __func__, asd->params.flash_state,
6394                                 asd->params.num_flash_frames);
6395                 return -EBUSY;
6396         }
6397
6398         asd->params.num_flash_frames = num_frames;
6399         asd->params.flash_state = ATOMISP_FLASH_REQUESTED;
6400         return 0;
6401 }
6402
6403 int atomisp_source_pad_to_stream_id(struct atomisp_sub_device *asd,
6404                                            uint16_t source_pad)
6405 {
6406         int stream_id;
6407         struct atomisp_device *isp = asd->isp;
6408
6409         if (isp->inputs[asd->input_curr].camera_caps->
6410                         sensor[asd->sensor_curr].stream_num == 1)
6411                 return ATOMISP_INPUT_STREAM_GENERAL;
6412
6413         switch (source_pad) {
6414         case ATOMISP_SUBDEV_PAD_SOURCE_CAPTURE:
6415                 stream_id = ATOMISP_INPUT_STREAM_CAPTURE;
6416                 break;
6417         case ATOMISP_SUBDEV_PAD_SOURCE_VF:
6418                 stream_id = ATOMISP_INPUT_STREAM_POSTVIEW;
6419                 break;
6420         case ATOMISP_SUBDEV_PAD_SOURCE_PREVIEW:
6421                 stream_id = ATOMISP_INPUT_STREAM_PREVIEW;
6422                 break;
6423         case ATOMISP_SUBDEV_PAD_SOURCE_VIDEO:
6424                 stream_id = ATOMISP_INPUT_STREAM_VIDEO;
6425                 break;
6426         default:
6427                 stream_id = ATOMISP_INPUT_STREAM_GENERAL;
6428         }
6429
6430         return stream_id;
6431 }
6432
6433 bool atomisp_is_vf_pipe(struct atomisp_video_pipe *pipe)
6434 {
6435         struct atomisp_sub_device *asd = pipe->asd;
6436
6437         if (pipe == &asd->video_out_vf)
6438                 return true;
6439
6440         if (asd->run_mode->val == ATOMISP_RUN_MODE_VIDEO &&
6441             pipe == &asd->video_out_preview)
6442                 return true;
6443
6444         return false;
6445 }
6446
6447 static int __checking_exp_id(struct atomisp_sub_device *asd, int exp_id)
6448 {
6449         struct atomisp_device *isp = asd->isp;
6450
6451         if (!asd->enable_raw_buffer_lock->val) {
6452                 dev_warn(isp->dev, "%s Raw Buffer Lock is disable.\n", __func__);
6453                 return -EINVAL;
6454         }
6455         if (asd->streaming != ATOMISP_DEVICE_STREAMING_ENABLED) {
6456                 dev_err(isp->dev, "%s streaming %d invalid exp_id %d.\n",
6457                         __func__, exp_id, asd->streaming);
6458                 return -EINVAL;
6459         }
6460         if ((exp_id > ATOMISP_MAX_EXP_ID) || (exp_id <= 0)) {
6461                 dev_err(isp->dev, "%s exp_id %d invalid.\n", __func__, exp_id);
6462                 return -EINVAL;
6463         }
6464         return 0;
6465 }
6466
6467 void atomisp_init_raw_buffer_bitmap(struct atomisp_sub_device *asd)
6468 {
6469         unsigned long flags;
6470         spin_lock_irqsave(&asd->raw_buffer_bitmap_lock, flags);
6471         memset(asd->raw_buffer_bitmap, 0, sizeof(asd->raw_buffer_bitmap));
6472         asd->raw_buffer_locked_count = 0;
6473         spin_unlock_irqrestore(&asd->raw_buffer_bitmap_lock, flags);
6474 }
6475
6476 int atomisp_set_raw_buffer_bitmap(struct atomisp_sub_device *asd, int exp_id)
6477 {
6478         int *bitmap, bit;
6479         unsigned long flags;
6480
6481         if (__checking_exp_id(asd, exp_id))
6482                 return -EINVAL;
6483
6484         bitmap = asd->raw_buffer_bitmap + exp_id / 32;
6485         bit = exp_id % 32;
6486         spin_lock_irqsave(&asd->raw_buffer_bitmap_lock, flags);
6487         (*bitmap) |= (1 << bit);
6488         asd->raw_buffer_locked_count++;
6489         spin_unlock_irqrestore(&asd->raw_buffer_bitmap_lock, flags);
6490
6491         dev_dbg(asd->isp->dev, "%s: exp_id %d,  raw_buffer_locked_count %d\n",
6492                 __func__, exp_id, asd->raw_buffer_locked_count);
6493
6494         /* Check if the raw buffer after next is still locked!!! */
6495         exp_id += 2;
6496         if (exp_id > ATOMISP_MAX_EXP_ID)
6497                 exp_id -= ATOMISP_MAX_EXP_ID;
6498         bitmap = asd->raw_buffer_bitmap + exp_id / 32;
6499         bit = exp_id % 32;
6500         if ((*bitmap) & (1 << bit)) {
6501                 int ret;
6502
6503                 /* WORKAROUND unlock the raw buffer compulsively */
6504                 ret = atomisp_css_exp_id_unlock(asd, exp_id);
6505                 if (ret) {
6506                         dev_err(asd->isp->dev, "%s exp_id is wrapping back to %d but force unlock failed,, err %d.\n",
6507                                 __func__, exp_id, ret);
6508                         return ret;
6509                 }
6510
6511                 spin_lock_irqsave(&asd->raw_buffer_bitmap_lock, flags);
6512                 (*bitmap) &= ~(1 << bit);
6513                 asd->raw_buffer_locked_count--;
6514                 spin_unlock_irqrestore(&asd->raw_buffer_bitmap_lock, flags);
6515                 dev_warn(asd->isp->dev, "%s exp_id is wrapping back to %d but it is still locked so force unlock it, raw_buffer_locked_count %d\n",
6516                         __func__, exp_id, asd->raw_buffer_locked_count);
6517         }
6518         return 0;
6519 }
6520
6521 static int __is_raw_buffer_locked(struct atomisp_sub_device *asd, int exp_id)
6522 {
6523         int *bitmap, bit;
6524         unsigned long flags;
6525         int ret;
6526
6527         if (__checking_exp_id(asd, exp_id))
6528                 return -EINVAL;
6529
6530         bitmap = asd->raw_buffer_bitmap + exp_id / 32;
6531         bit = exp_id % 32;
6532         spin_lock_irqsave(&asd->raw_buffer_bitmap_lock, flags);
6533         ret = ((*bitmap) & (1 << bit));
6534         spin_unlock_irqrestore(&asd->raw_buffer_bitmap_lock, flags);
6535         return !ret;
6536 }
6537
6538 static int __clear_raw_buffer_bitmap(struct atomisp_sub_device *asd, int exp_id)
6539 {
6540         int *bitmap, bit;
6541         unsigned long flags;
6542
6543         if (__is_raw_buffer_locked(asd, exp_id))
6544                 return -EINVAL;
6545
6546         bitmap = asd->raw_buffer_bitmap + exp_id / 32;
6547         bit = exp_id % 32;
6548         spin_lock_irqsave(&asd->raw_buffer_bitmap_lock, flags);
6549         (*bitmap) &= ~(1 << bit);
6550         asd->raw_buffer_locked_count--;
6551         spin_unlock_irqrestore(&asd->raw_buffer_bitmap_lock, flags);
6552
6553         dev_dbg(asd->isp->dev, "%s: exp_id %d,  raw_buffer_locked_count %d\n",
6554                 __func__, exp_id, asd->raw_buffer_locked_count);
6555         return 0;
6556 }
6557
6558 int atomisp_exp_id_capture(struct atomisp_sub_device *asd, int *exp_id)
6559 {
6560         struct atomisp_device *isp = asd->isp;
6561         int value = *exp_id;
6562         int ret;
6563
6564         ret = __is_raw_buffer_locked(asd, value);
6565         if (ret) {
6566                 dev_err(isp->dev, "%s exp_id %d invalid %d.\n", __func__, value, ret);
6567                 return -EINVAL;
6568         }
6569
6570         dev_dbg(isp->dev, "%s exp_id %d\n", __func__, value);
6571         ret = atomisp_css_exp_id_capture(asd, value);
6572         if (ret) {
6573                 dev_err(isp->dev, "%s exp_id %d failed.\n", __func__, value);
6574                 return -EIO;
6575         }
6576         return 0;
6577 }
6578
6579 int atomisp_exp_id_unlock(struct atomisp_sub_device *asd, int *exp_id)
6580 {
6581         struct atomisp_device *isp = asd->isp;
6582         int value = *exp_id;
6583         int ret;
6584
6585         ret = __clear_raw_buffer_bitmap(asd, value);
6586         if (ret) {
6587                 dev_err(isp->dev, "%s exp_id %d invalid %d.\n", __func__, value, ret);
6588                 return -EINVAL;
6589         }
6590
6591         dev_dbg(isp->dev, "%s exp_id %d\n", __func__, value);
6592         ret = atomisp_css_exp_id_unlock(asd, value);
6593         if (ret)
6594                 dev_err(isp->dev, "%s exp_id %d failed, err %d.\n",
6595                         __func__, value, ret);
6596
6597         return ret;
6598 }
6599
6600 int atomisp_enable_dz_capt_pipe(struct atomisp_sub_device *asd,
6601                                            unsigned int *enable)
6602 {
6603         bool value;
6604
6605         if (enable == NULL)
6606                 return -EINVAL;
6607
6608         value = *enable > 0 ? true : false;
6609
6610         atomisp_en_dz_capt_pipe(asd, value);
6611
6612         return 0;
6613 }
6614
6615 int atomisp_inject_a_fake_event(struct atomisp_sub_device *asd, int *event)
6616 {
6617         if (!event || asd->streaming != ATOMISP_DEVICE_STREAMING_ENABLED)
6618                 return -EINVAL;
6619
6620         dev_dbg(asd->isp->dev, "%s: trying to inject a fake event 0x%x\n",
6621                 __func__, *event);
6622
6623         switch (*event) {
6624         case V4L2_EVENT_FRAME_SYNC:
6625                 atomisp_sof_event(asd);
6626                 break;
6627         case V4L2_EVENT_FRAME_END:
6628                 atomisp_eof_event(asd, 0);
6629                 break;
6630         case V4L2_EVENT_ATOMISP_3A_STATS_READY:
6631                 atomisp_3a_stats_ready_event(asd, 0);
6632                 break;
6633         case V4L2_EVENT_ATOMISP_METADATA_READY:
6634                 atomisp_metadata_ready_event(asd, 0);
6635                 break;
6636         default:
6637                 return -EINVAL;
6638         }
6639
6640         return 0;
6641 }
6642
6643 int atomisp_get_pipe_id(struct atomisp_video_pipe *pipe)
6644 {
6645         struct atomisp_sub_device *asd = pipe->asd;
6646
6647         if (ATOMISP_USE_YUVPP(asd))
6648                 return CSS_PIPE_ID_YUVPP;
6649         else if (asd->vfpp->val == ATOMISP_VFPP_DISABLE_SCALER)
6650                 return CSS_PIPE_ID_VIDEO;
6651         else if (asd->vfpp->val == ATOMISP_VFPP_DISABLE_LOWLAT)
6652                 return CSS_PIPE_ID_CAPTURE;
6653         else if (pipe == &asd->video_out_video_capture)
6654                 return CSS_PIPE_ID_VIDEO;
6655         else if (pipe == &asd->video_out_vf)
6656                 return CSS_PIPE_ID_CAPTURE;
6657         else if (pipe == &asd->video_out_preview) {
6658                 if (asd->run_mode->val == ATOMISP_RUN_MODE_VIDEO)
6659                         return CSS_PIPE_ID_VIDEO;
6660                 else
6661                         return CSS_PIPE_ID_PREVIEW;
6662         } else if (pipe == &asd->video_out_capture) {
6663                 if (asd->copy_mode)
6664                         return IA_CSS_PIPE_ID_COPY;
6665                 else
6666                         return CSS_PIPE_ID_CAPTURE;
6667         }
6668
6669         /* fail through */
6670         dev_warn(asd->isp->dev, "%s failed to find proper pipe\n",
6671                 __func__);
6672         return CSS_PIPE_ID_CAPTURE;
6673 }
6674
6675 int atomisp_get_invalid_frame_num(struct video_device *vdev,
6676                                         int *invalid_frame_num)
6677 {
6678         struct atomisp_video_pipe *pipe = atomisp_to_video_pipe(vdev);
6679         struct atomisp_sub_device *asd = pipe->asd;
6680         enum atomisp_css_pipe_id pipe_id;
6681         struct ia_css_pipe_info p_info;
6682         int ret;
6683
6684         if (asd->isp->inputs[asd->input_curr].camera_caps->
6685                 sensor[asd->sensor_curr].stream_num > 1) {
6686                 /* External ISP */
6687                 *invalid_frame_num = 0;
6688                 return 0;
6689         }
6690
6691         pipe_id = atomisp_get_pipe_id(pipe);
6692         if (!asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL].pipes[pipe_id]) {
6693                 dev_warn(asd->isp->dev, "%s pipe %d has not been created yet, do SET_FMT first!\n",
6694                         __func__, pipe_id);
6695                 return -EINVAL;
6696         }
6697
6698         ret = ia_css_pipe_get_info(
6699                 asd->stream_env[ATOMISP_INPUT_STREAM_GENERAL]
6700                 .pipes[pipe_id], &p_info);
6701         if (ret == IA_CSS_SUCCESS) {
6702                 *invalid_frame_num = p_info.num_invalid_frames;
6703                 return 0;
6704         } else {
6705                 dev_warn(asd->isp->dev, "%s get pipe infor failed %d\n",
6706                          __func__, ret);
6707                 return -EINVAL;
6708         }
6709 }