drivers/media/platform/starfive/v4l2_driver/stf_vin.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /*
   3  * Copyright (C) 2021 StarFive Technology Co., Ltd.
   4  */
   5 #include <linux/kernel.h>
   6 #include <linux/module.h>
   7 #include <linux/interrupt.h>
   8 #include <linux/dma-mapping.h>
   9
  10 #include "stfcamss.h"
  11
  12 #define vin_line_array(ptr_line) \
  13                 ((const struct vin_line (*)[]) &(ptr_line[-(ptr_line->id)]))
  14
  15 #define line_to_vin2_dev(ptr_line) \
  16                 container_of(vin_line_array(ptr_line), struct stf_vin2_dev, line)
  17
  18 #define VIN_FRAME_DROP_MAX_VAL 30
  19 #define VIN_FRAME_DROP_MIN_VAL 4
  20
  21 // #define VIN_TWO_BUFFER
  22
  23 static const struct vin2_format vin2_formats_st7110[] = {
  24         { MEDIA_BUS_FMT_YUYV8_2X8, 16},
  25         { MEDIA_BUS_FMT_RGB565_2X8_LE, 16},
  26         { MEDIA_BUS_FMT_SRGGB8_1X8, 8},
  27         { MEDIA_BUS_FMT_SGRBG8_1X8, 8},
  28         { MEDIA_BUS_FMT_SGBRG8_1X8, 8},
  29         { MEDIA_BUS_FMT_SBGGR8_1X8, 8},
  30         { MEDIA_BUS_FMT_SRGGB10_1X10, 10},
  31         { MEDIA_BUS_FMT_SGRBG10_1X10, 10},
  32         { MEDIA_BUS_FMT_SGBRG10_1X10, 10},
  33         { MEDIA_BUS_FMT_SBGGR10_1X10, 10},
  34         { MEDIA_BUS_FMT_SRGGB12_1X12, 12},
  35         { MEDIA_BUS_FMT_SGRBG12_1X12, 12},
  36         { MEDIA_BUS_FMT_SGBRG12_1X12, 12},
  37         { MEDIA_BUS_FMT_SBGGR12_1X12, 12},
  38         { MEDIA_BUS_FMT_Y12_1X12, 8},
  39         { MEDIA_BUS_FMT_YUV8_1X24, 8},
  40 };
  41
  42 static const struct vin2_format isp_formats_st7110_raw[] = {
  43         { MEDIA_BUS_FMT_SBGGR12_1X12, 12},
  44         { MEDIA_BUS_FMT_SRGGB12_1X12, 12},
  45         { MEDIA_BUS_FMT_SGRBG12_1X12, 12},
  46         { MEDIA_BUS_FMT_SGBRG12_1X12, 12},
  47 };
  48
  49 static const struct vin2_format isp_formats_st7110_uo[] = {
  50         { MEDIA_BUS_FMT_Y12_1X12, 8},
  51 };
  52
  53 static const struct vin2_format isp_formats_st7110_iti[] = {
  54         { MEDIA_BUS_FMT_SRGGB10_1X10, 10},
  55         { MEDIA_BUS_FMT_SGRBG10_1X10, 10},
  56         { MEDIA_BUS_FMT_SGBRG10_1X10, 10},
  57         { MEDIA_BUS_FMT_SBGGR10_1X10, 10},
  58         { MEDIA_BUS_FMT_SRGGB12_1X12, 12},
  59         { MEDIA_BUS_FMT_SGRBG12_1X12, 12},
  60         { MEDIA_BUS_FMT_SGBRG12_1X12, 12},
  61         { MEDIA_BUS_FMT_SBGGR12_1X12, 12},
  62         { MEDIA_BUS_FMT_Y12_1X12, 8},
  63         { MEDIA_BUS_FMT_YUV8_1X24, 8},
  64 };
  65
  66 static const struct vin2_format_table vin2_formats_table[] = {
  67         /* VIN_LINE_WR */
  68         { vin2_formats_st7110, ARRAY_SIZE(vin2_formats_st7110) },
  69         /* VIN_LINE_ISP */
  70         { isp_formats_st7110_uo, ARRAY_SIZE(isp_formats_st7110_uo) },
  71         /* VIN_LINE_ISP_SS0 */
  72         { isp_formats_st7110_uo, ARRAY_SIZE(isp_formats_st7110_uo) },
  73         /* VIN_LINE_ISP_SS1 */
  74         { isp_formats_st7110_uo, ARRAY_SIZE(isp_formats_st7110_uo) },
  75         /* VIN_LINE_ISP_ITIW */
  76         { isp_formats_st7110_iti, ARRAY_SIZE(isp_formats_st7110_iti) },
  77         /* VIN_LINE_ISP_ITIR */
  78         { isp_formats_st7110_iti, ARRAY_SIZE(isp_formats_st7110_iti) },
  79         /* VIN_LINE_ISP_RAW */
  80         { isp_formats_st7110_raw, ARRAY_SIZE(isp_formats_st7110_raw) },
  81         /* VIN_LINE_ISP_SCD_Y */
  82         { isp_formats_st7110_raw, ARRAY_SIZE(isp_formats_st7110_raw) },
  83 };
  84
  85 static void vin_buffer_done(struct vin_line *line, struct vin_params *params);
  86 static void vin_change_buffer(struct vin_line *line);
  87 static struct stfcamss_buffer *vin_buf_get_pending(struct vin_output *output);
  88 static void vin_output_init_addrs(struct vin_line *line);
  89 static void vin_init_outputs(struct vin_line *line);
  90 static struct v4l2_mbus_framefmt *
  91 __vin_get_format(struct vin_line *line,
  92                 struct v4l2_subdev_state *state,
  93                 unsigned int pad,
  94                 enum v4l2_subdev_format_whence which);
  95
  96 static char *get_line_subdevname(int line_id)
  97 {
  98         char *name = NULL;
  99
 100         switch (line_id) {
 101         case VIN_LINE_WR:
 102                 name = "wr";
 103                 break;
 104         case VIN_LINE_ISP:
 105                 name = "isp0";
 106                 break;
 107         case VIN_LINE_ISP_SS0:
 108                 name = "isp0_ss0";
 109                 break;
 110         case VIN_LINE_ISP_SS1:
 111                 name = "isp0_ss1";
 112                 break;
 113         case VIN_LINE_ISP_ITIW:
 114                 name = "isp0_itiw";
 115                 break;
 116         case VIN_LINE_ISP_ITIR:
 117                 name = "isp0_itir";
 118                 break;
 119         case VIN_LINE_ISP_RAW:
 120                 name = "isp0_raw";
 121                 break;
 122         case VIN_LINE_ISP_SCD_Y:
 123                 name = "isp0_scd_y";
 124                 break;
 125         default:
 126                 name = "unknow";
 127                 break;
 128         }
 129         return name;
 130 }
 131
 132 static enum isp_line_id stf_vin_map_isp_line(enum vin_line_id line)
 133 {
 134         enum isp_line_id line_id;
 135
 136         if ((line > VIN_LINE_WR) && (line < VIN_LINE_MAX)) {
 137                 line_id = line % STF_ISP_LINE_SRC_SCD_Y;
 138                 if (line_id == 0)
 139                         line_id = line_id ? line_id : STF_ISP_LINE_SRC_SCD_Y;
 140         } else
 141                 line_id = STF_ISP_LINE_INVALID;
 142
 143         return line_id;
 144 }
 145
 146 enum isp_pad_id stf_vin_map_isp_pad(enum vin_line_id line, enum isp_pad_id def)
 147 {
 148         enum isp_pad_id pad_id;
 149
 150         if (line == VIN_LINE_WR)
 151                 pad_id = STF_ISP_PAD_SINK;
 152         else if ((line > VIN_LINE_WR) && (line < VIN_LINE_MAX))
 153                 pad_id = stf_vin_map_isp_line(line);
 154         else
 155                 pad_id = def;
 156
 157         return pad_id;
 158 }
 159
 160 int stf_vin_subdev_init(struct stfcamss *stfcamss)
 161 {
 162         struct stf_vin_dev *vin;
 163         struct device *dev = stfcamss->dev;
 164         struct stf_vin2_dev *vin_dev = stfcamss->vin_dev;
 165         int i, ret = 0;
 166
 167         vin_dev->stfcamss = stfcamss;
 168         vin_dev->hw_ops = &vin_ops;
 169         vin_dev->hw_ops->isr_buffer_done = vin_buffer_done;
 170         vin_dev->hw_ops->isr_change_buffer = vin_change_buffer;
 171
 172         vin = stfcamss->vin;
 173         atomic_set(&vin_dev->ref_count, 0);
 174
 175         ret = devm_request_irq(dev,
 176                         vin->irq, vin_dev->hw_ops->vin_wr_irq_handler,
 177                         0, "vin_axiwr_irq", vin_dev);
 178         if (ret) {
 179                 st_err(ST_VIN, "failed to request irq\n");
 180                 goto out;
 181         }
 182
 183         ret = devm_request_irq(dev,
 184                         vin->isp_irq, vin_dev->hw_ops->vin_isp_irq_handler,
 185                         0, "vin_isp_irq", vin_dev);
 186         if (ret) {
 187                 st_err(ST_VIN, "failed to request isp irq\n");
 188                 goto out;
 189         }
 190
 191         st_info(ST_CAMSS, "%s, %d!\n", __func__, __LINE__);
 192 #ifdef ISP_USE_CSI_AND_SC_DONE_INTERRUPT
 193         ret = devm_request_irq(dev,
 194                         vin->isp_csi_irq, vin_dev->hw_ops->vin_isp_csi_irq_handler,
 195                         0, "vin_isp_csi_irq", vin_dev);
 196         if (ret) {
 197                 st_err(ST_VIN, "failed to request isp raw irq\n");
 198                 goto out;
 199         }
 200
 201         ret = devm_request_irq(dev,
 202                         vin->isp_scd_irq, vin_dev->hw_ops->vin_isp_scd_irq_handler,
 203                         0, "vin_isp_scd_irq", vin_dev);
 204         if (ret) {
 205                 st_err(ST_VIN, "failed to request isp scd irq\n");
 206                 goto out;
 207         }
 208 #endif
 209
 210         ret = devm_request_irq(dev,
 211                         vin->isp_irq_csiline, vin_dev->hw_ops->vin_isp_irq_csiline_handler,
 212                         0, "vin_isp_irq_csiline", vin_dev);
 213         if (ret) {
 214                 st_err(ST_VIN, "failed to request isp irq csiline\n");
 215                 goto out;
 216         }
 217
 218         vin_dev->hw_ops->vin_wr_irq_enable(vin_dev, 1);
 219         vin_dev->hw_ops->vin_wr_irq_enable(vin_dev, 0);
 220
 221         /* Reset device */
 222         /*Do not configure the CLK before powering on the device,
 223          *add vin_power_on() to vin_set_power() 2021 1111
 224          */
 225         ret = vin_dev->hw_ops->vin_top_clk_init(vin_dev);
 226         if (ret) {
 227                 st_err(ST_VIN, "Failed to reset device\n");
 228                 goto out;
 229         }
 230
 231         // /* set the sysctl config */
 232         // ret = vin_dev->hw_ops->vin_config_set(vin_dev);
 233         // if (ret) {
 234         //      st_err(ST_VIN, "Failed to config device\n");
 235         //      goto out;
 236         // }
 237
 238         mutex_init(&vin_dev->power_lock);
 239         vin_dev->power_count = 0;
 240
 241         for (i = 0; i < STF_DUMMY_MODULE_NUMS; i++) {
 242                 struct dummy_buffer *dummy_buffer = &vin_dev->dummy_buffer[i];
 243
 244                 mutex_init(&dummy_buffer->stream_lock);
 245                 dummy_buffer->nums = i == 0 ? VIN_DUMMY_BUFFER_NUMS : ISP_DUMMY_BUFFER_NUMS;
 246                 dummy_buffer->stream_count = 0;
 247                 dummy_buffer->buffer = devm_kzalloc(dev,
 248                         dummy_buffer->nums * sizeof(struct vin_dummy_buffer), GFP_KERNEL);
 249                 atomic_set(&dummy_buffer->frame_skip, 0);
 250         }
 251
 252         for (i = VIN_LINE_WR;
 253                 i < STF_ISP_LINE_MAX + 1; i++) {
 254                 struct vin_line *l = &vin_dev->line[i];
 255                 int is_mp;
 256
 257                 is_mp = i == VIN_LINE_WR ? false : true;
 258                 is_mp = false;
 259                 if (stf_vin_map_isp_line(i) == STF_ISP_LINE_SRC_ITIR)
 260                         l->video_out.type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
 261                 else
 262                         l->video_out.type = is_mp ? V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE :
 263                                 V4L2_BUF_TYPE_VIDEO_CAPTURE;
 264                 l->video_out.stfcamss = stfcamss;
 265                 l->id = i;
 266                 l->sdev_type = VIN_DEV_TYPE;
 267                 l->formats = vin2_formats_table[i].fmts;
 268                 l->nformats = vin2_formats_table[i].nfmts;
 269                 spin_lock_init(&l->output_lock);
 270
 271                 mutex_init(&l->stream_lock);
 272                 l->stream_count = 0;
 273                 mutex_init(&l->power_lock);
 274                 l->power_count = 0;
 275         }
 276
 277         return 0;
 278 out:
 279         return ret;
 280 }
 281
 282 static int vin_set_power(struct v4l2_subdev *sd, int on)
 283 {
 284         struct vin_line *line = v4l2_get_subdevdata(sd);
 285         struct stf_vin2_dev *vin_dev = line_to_vin2_dev(line);
 286
 287         mutex_lock(&line->power_lock);
 288         if (on) {
 289                 if (line->power_count == 0)
 290                         vin_init_outputs(line);
 291                 line->power_count++;
 292         } else {
 293                 if (line->power_count == 0) {
 294                         st_err(ST_VIN,
 295                                 "line power off on power_count == 0\n");
 296                         goto exit_line;
 297                 }
 298                 line->power_count--;
 299         }
 300 exit_line:
 301         mutex_unlock(&line->power_lock);
 302
 303         mutex_lock(&vin_dev->power_lock);
 304         if (on) {
 305                 if (vin_dev->power_count == 0) {
 306                         //vin_dev->hw_ops->vin_top_clk_init(vin_dev);
 307                         vin_dev->hw_ops->vin_clk_enable(vin_dev);
 308                         vin_dev->hw_ops->vin_config_set(vin_dev);
 309                 }
 310                 vin_dev->power_count++;
 311         } else {
 312                 if (vin_dev->power_count == 0) {
 313                         st_err(ST_VIN,
 314                                 "vin_dev power off on power_count == 0\n");
 315                         goto exit;
 316                 }
 317                 if (vin_dev->power_count == 1) {
 318                         vin_dev->hw_ops->vin_clk_disable(vin_dev);
 319                         //vin_dev->hw_ops->vin_top_clk_deinit(vin_dev);
 320                 }
 321                 vin_dev->power_count--;
 322         }
 323 exit:
 324
 325         mutex_unlock(&vin_dev->power_lock);
 326
 327         return 0;
 328 }
 329
 330 static unsigned int get_frame_skip(struct vin_line *line)
 331 {
 332         unsigned int frame_skip = 0;
 333         struct media_entity *sensor;
 334
 335         sensor = stfcamss_find_sensor(&line->subdev.entity);
 336         if (sensor) {
 337                 struct v4l2_subdev *subdev =
 338                                         media_entity_to_v4l2_subdev(sensor);
 339
 340                 v4l2_subdev_call(subdev, sensor, g_skip_frames, &frame_skip);
 341                 frame_skip += VIN_FRAME_DROP_MIN_VAL;
 342                 if (frame_skip > VIN_FRAME_DROP_MAX_VAL)
 343                         frame_skip = VIN_FRAME_DROP_MAX_VAL;
 344                 st_debug(ST_VIN, "%s, frame_skip %d\n", __func__, frame_skip);
 345         }
 346
 347         return frame_skip;
 348 }
 349
 350 static int vin_enable_output(struct vin_line *line)
 351 {
 352         struct vin_output *output = &line->output;
 353         unsigned long flags;
 354
 355         spin_lock_irqsave(&line->output_lock, flags);
 356
 357         output->state = VIN_OUTPUT_IDLE;
 358
 359         output->buf[0] = vin_buf_get_pending(output);
 360 #ifdef VIN_TWO_BUFFER
 361         if (line->id == VIN_LINE_WR)
 362                 output->buf[1] = vin_buf_get_pending(output);
 363 #endif
 364         if (!output->buf[0] && output->buf[1]) {
 365                 output->buf[0] = output->buf[1];
 366                 output->buf[1] = NULL;
 367         }
 368
 369         if (output->buf[0])
 370                 output->state = VIN_OUTPUT_SINGLE;
 371
 372 #ifdef VIN_TWO_BUFFER
 373         if (output->buf[1] && line->id == VIN_LINE_WR)
 374                 output->state = VIN_OUTPUT_CONTINUOUS;
 375 #endif
 376         output->sequence = 0;
 377
 378         vin_output_init_addrs(line);
 379         spin_unlock_irqrestore(&line->output_lock, flags);
 380         return 0;
 381 }
 382
 383 static int vin_disable_output(struct vin_line *line)
 384 {
 385         struct vin_output *output = &line->output;
 386         unsigned long flags;
 387
 388         spin_lock_irqsave(&line->output_lock, flags);
 389
 390         output->state = VIN_OUTPUT_OFF;
 391
 392         spin_unlock_irqrestore(&line->output_lock, flags);
 393         return 0;
 394 }
 395
 396 static u32 line_to_dummy_module(struct vin_line *line)
 397 {
 398         u32 dummy_module = 0;
 399
 400         switch (line->id) {
 401         case VIN_LINE_WR:
 402                 dummy_module = STF_DUMMY_VIN;
 403                 break;
 404         case VIN_LINE_ISP:
 405         case VIN_LINE_ISP_SS0:
 406         case VIN_LINE_ISP_SS1:
 407         case VIN_LINE_ISP_ITIW:
 408         case VIN_LINE_ISP_ITIR:
 409         case VIN_LINE_ISP_RAW:
 410         case VIN_LINE_ISP_SCD_Y:
 411                 dummy_module = STF_DUMMY_ISP;
 412                 break;
 413         default:
 414                 dummy_module = STF_DUMMY_VIN;
 415                 break;
 416         }
 417
 418         return dummy_module;
 419 }
 420
 421 static int vin_alloc_dummy_buffer(struct stf_vin2_dev *vin_dev,
 422                 struct v4l2_mbus_framefmt *fmt, int dummy_module)
 423 {
 424         struct device *dev = vin_dev->stfcamss->dev;
 425         struct dummy_buffer *dummy_buffer = &vin_dev->dummy_buffer[dummy_module];
 426         struct vin_dummy_buffer *buffer = NULL;
 427         int ret = 0, i;
 428         u32 aligns;
 429
 430         for (i = 0; i < dummy_buffer->nums; i++) {
 431                 buffer = &vin_dev->dummy_buffer[dummy_module].buffer[i];
 432                 buffer->width = fmt->width;
 433                 buffer->height = fmt->height;
 434                 buffer->mcode = fmt->code;
 435                 if (i == STF_VIN_PAD_SINK) {
 436                         aligns = ALIGN(fmt->width * 4, STFCAMSS_FRAME_WIDTH_ALIGN_8);
 437                         buffer->buffer_size = PAGE_ALIGN(aligns * fmt->height);
 438                 } else if (i == STF_ISP_PAD_SRC
 439                         || i == STF_ISP_PAD_SRC_SS0
 440                         || i == STF_ISP_PAD_SRC_SS1) {
 441                         aligns = ALIGN(fmt->width, STFCAMSS_FRAME_WIDTH_ALIGN_8);
 442                         buffer->buffer_size = PAGE_ALIGN(aligns * fmt->height * 3 / 2);
 443                 } else if (i == STF_ISP_PAD_SRC_ITIW
 444                         || i == STF_ISP_PAD_SRC_ITIR) {
 445                         aligns = ALIGN(fmt->width, STFCAMSS_FRAME_WIDTH_ALIGN_8);
 446                         buffer->buffer_size = PAGE_ALIGN(aligns * fmt->height * 3);
 447                 } else if (i == STF_ISP_PAD_SRC_RAW) {
 448                         aligns = ALIGN(fmt->width * ISP_RAW_DATA_BITS / 8,
 449                                         STFCAMSS_FRAME_WIDTH_ALIGN_128);
 450                         buffer->buffer_size = PAGE_ALIGN(aligns * fmt->height);
 451                 } else if (i == STF_ISP_PAD_SRC_SCD_Y)
 452                         buffer->buffer_size = PAGE_ALIGN(ISP_SCD_Y_BUFFER_SIZE);
 453                 else
 454                         continue;
 455
 456                 buffer->vaddr = dma_alloc_coherent(dev, buffer->buffer_size,
 457                                 &buffer->paddr[0], GFP_DMA | GFP_KERNEL);
 458
 459                 if (buffer->vaddr) {
 460                         if (i == STF_ISP_PAD_SRC
 461                                 || i == STF_ISP_PAD_SRC_SS0
 462                                 || i == STF_ISP_PAD_SRC_SS1
 463                                 || i == STF_ISP_PAD_SRC_ITIW
 464                                 || i == STF_ISP_PAD_SRC_ITIR)
 465                                 buffer->paddr[1] = (dma_addr_t)(buffer->paddr[0] +
 466                                                                         aligns * fmt->height);
 467                         else if (i == STF_ISP_PAD_SRC_SCD_Y)
 468                                 buffer->paddr[1] = (dma_addr_t)(buffer->paddr[0] +
 469                                                                         ISP_YHIST_BUFFER_SIZE);
 470                         else
 471                                 st_debug(ST_VIN, "signal plane\n");
 472                 }
 473                 {
 474                         char szPadName[][32] = {
 475                                 "VIN_PAD_SINK",
 476                                 "ISP_PAD_SRC",
 477                                 "ISP_PAD_SRC_SS0",
 478                                 "ISP_PAD_SRC_SS1",
 479                                 "ISP_PAD_SRC_ITIW",
 480                                 "ISP_PAD_SRC_ITIR",
 481                                 "ISP_PAD_SRC_RAW",
 482                                 "ISP_PAD_SRC_SCD_Y",
 483                                 "Unknown Pad"
 484                         };
 485
 486                         st_debug(ST_VIN, "%s: i = %d(%s) addr[0] = %llx, addr[1] = %llx, size = %u bytes\n",
 487                                 __func__,
 488                                 i,
 489                                 szPadName[i],
 490                                 buffer->paddr[0],
 491                                 buffer->paddr[1],
 492                                 buffer->buffer_size
 493                                 );
 494                 }
 495         }
 496
 497         return ret;
 498 }
 499
 500 static void vin_free_dummy_buffer(struct stf_vin2_dev *vin_dev, int dummy_module)
 501 {
 502         struct device *dev = vin_dev->stfcamss->dev;
 503         struct dummy_buffer *dummy_buffer = &vin_dev->dummy_buffer[dummy_module];
 504         struct vin_dummy_buffer *buffer = NULL;
 505         int i;
 506
 507         for (i = 0; i < dummy_buffer->nums; i++) {
 508                 buffer = &dummy_buffer->buffer[i];
 509                 if (buffer->vaddr)
 510                         dma_free_coherent(dev, buffer->buffer_size,
 511                                                 buffer->vaddr, buffer->paddr[0]);
 512                 memset(buffer, 0, sizeof(struct vin_dummy_buffer));
 513         }
 514 }
 515
 516 static void vin_set_dummy_buffer(struct vin_line *line, u32 pad)
 517 {
 518         struct stf_vin2_dev *vin_dev = line_to_vin2_dev(line);
 519         int dummy_module = line_to_dummy_module(line);
 520         struct dummy_buffer *dummy_buffer = &vin_dev->dummy_buffer[dummy_module];
 521         struct vin_dummy_buffer *buffer = NULL;
 522
 523         switch (pad) {
 524         case STF_VIN_PAD_SINK:
 525                 if (line->id == VIN_LINE_WR) {
 526                         buffer = &dummy_buffer->buffer[STF_VIN_PAD_SINK];
 527                         vin_dev->hw_ops->vin_wr_set_ping_addr(vin_dev, buffer->paddr[0]);
 528                         vin_dev->hw_ops->vin_wr_set_pong_addr(vin_dev, buffer->paddr[0]);
 529                 } else {
 530                         buffer = &dummy_buffer->buffer[STF_ISP_PAD_SRC];
 531                         vin_dev->hw_ops->vin_isp_set_yuv_addr(vin_dev,
 532                                 buffer->paddr[0], buffer->paddr[1]);
 533
 534                         buffer = &dummy_buffer->buffer[STF_ISP_PAD_SRC_SS0];
 535                         vin_dev->hw_ops->vin_isp_set_ss0_addr(vin_dev,
 536                                 buffer->paddr[0], buffer->paddr[1]);
 537
 538                         buffer = &dummy_buffer->buffer[STF_ISP_PAD_SRC_SS1];
 539                         vin_dev->hw_ops->vin_isp_set_ss1_addr(vin_dev,
 540                                 buffer->paddr[0], buffer->paddr[1]);
 541
 542                         buffer = &dummy_buffer->buffer[STF_ISP_PAD_SRC_ITIW];
 543                         vin_dev->hw_ops->vin_isp_set_itiw_addr(vin_dev,
 544                                 buffer->paddr[0], buffer->paddr[1]);
 545
 546                         buffer = &dummy_buffer->buffer[STF_ISP_PAD_SRC_ITIR];
 547                         vin_dev->hw_ops->vin_isp_set_itir_addr(vin_dev,
 548                                 buffer->paddr[0], buffer->paddr[1]);
 549
 550                         buffer = &dummy_buffer->buffer[STF_ISP_PAD_SRC_RAW];
 551                         vin_dev->hw_ops->vin_isp_set_raw_addr(vin_dev, buffer->paddr[0]);
 552
 553                         buffer = &dummy_buffer->buffer[STF_ISP_PAD_SRC_SCD_Y];
 554                         vin_dev->hw_ops->vin_isp_set_scd_addr(vin_dev,
 555                                 buffer->paddr[0], buffer->paddr[1], AWB_TYPE);
 556                 }
 557                 break;
 558         case STF_ISP_PAD_SRC:
 559                 buffer = &dummy_buffer->buffer[STF_ISP_PAD_SRC];
 560                 vin_dev->hw_ops->vin_isp_set_yuv_addr(vin_dev,
 561                         buffer->paddr[0], buffer->paddr[1]);
 562                 break;
 563         case STF_ISP_PAD_SRC_SS0:
 564                 buffer = &dummy_buffer->buffer[STF_ISP_PAD_SRC_SS0];
 565                 vin_dev->hw_ops->vin_isp_set_ss0_addr(vin_dev,
 566                         buffer->paddr[0], buffer->paddr[1]);
 567                 break;
 568         case STF_ISP_PAD_SRC_SS1:
 569                 buffer = &dummy_buffer->buffer[STF_ISP_PAD_SRC_SS1];
 570                 vin_dev->hw_ops->vin_isp_set_ss1_addr(vin_dev,
 571                         buffer->paddr[0], buffer->paddr[1]);
 572                 break;
 573         case STF_ISP_PAD_SRC_ITIW:
 574                 buffer = &dummy_buffer->buffer[STF_ISP_PAD_SRC_ITIW];
 575                 vin_dev->hw_ops->vin_isp_set_itiw_addr(vin_dev,
 576                         buffer->paddr[0], buffer->paddr[1]);
 577                 break;
 578         case STF_ISP_PAD_SRC_ITIR:
 579                 buffer = &dummy_buffer->buffer[STF_ISP_PAD_SRC_ITIR];
 580                 vin_dev->hw_ops->vin_isp_set_itir_addr(vin_dev,
 581                         buffer->paddr[0], buffer->paddr[1]);
 582                 break;
 583         case STF_ISP_PAD_SRC_RAW:
 584                 buffer = &dummy_buffer->buffer[STF_ISP_PAD_SRC_RAW];
 585                 vin_dev->hw_ops->vin_isp_set_raw_addr(vin_dev, buffer->paddr[0]);
 586                 break;
 587         case STF_ISP_PAD_SRC_SCD_Y:
 588                 buffer = &dummy_buffer->buffer[STF_ISP_PAD_SRC_SCD_Y];
 589                 vin_dev->hw_ops->vin_isp_set_scd_addr(vin_dev,
 590                         buffer->paddr[0], buffer->paddr[1], AWB_TYPE);
 591                 break;
 592         default:
 593                 break;
 594         }
 595 }
 596
 597 static int vin_set_stream(struct v4l2_subdev *sd, int enable)
 598 {
 599         struct vin_line *line = v4l2_get_subdevdata(sd);
 600         struct stf_vin2_dev *vin_dev = line_to_vin2_dev(line);
 601         int dummy_module = line_to_dummy_module(line);
 602         struct dummy_buffer *dummy_buffer = &vin_dev->dummy_buffer[dummy_module];
 603         struct v4l2_mbus_framefmt *fmt;
 604
 605         st_debug(ST_VIN, "%s, %d\n", __func__, __LINE__);
 606         fmt = __vin_get_format(line, NULL, STF_VIN_PAD_SINK, V4L2_SUBDEV_FORMAT_ACTIVE);
 607         mutex_lock(&dummy_buffer->stream_lock);
 608         if (enable) {
 609                 if (dummy_buffer->stream_count == 0) {
 610                         vin_alloc_dummy_buffer(vin_dev, fmt, dummy_module);
 611                         vin_set_dummy_buffer(line, STF_VIN_PAD_SINK);
 612                         atomic_set(&dummy_buffer->frame_skip, get_frame_skip(line));
 613                 }
 614                 dummy_buffer->stream_count++;
 615         } else {
 616                 if (dummy_buffer->stream_count == 1) {
 617                         vin_free_dummy_buffer(vin_dev, dummy_module);
 618                         // set buffer addr to zero
 619                         vin_set_dummy_buffer(line, STF_VIN_PAD_SINK);
 620                 } else
 621                         vin_set_dummy_buffer(line,
 622                                         stf_vin_map_isp_pad(line->id, STF_ISP_PAD_SINK));
 623
 624                 dummy_buffer->stream_count--;
 625         }
 626         mutex_unlock(&dummy_buffer->stream_lock);
 627
 628         if (line->id == VIN_LINE_WR) {
 629                 mutex_lock(&line->stream_lock);
 630                 if (enable) {
 631                         if (line->stream_count == 0) {
 632                                 vin_dev->hw_ops->vin_wr_irq_enable(vin_dev, 1);
 633                                 vin_dev->hw_ops->vin_wr_stream_set(vin_dev, 1);
 634                         }
 635                         line->stream_count++;
 636                 } else {
 637                         if (line->stream_count == 1) {
 638                                 vin_dev->hw_ops->vin_wr_irq_enable(vin_dev, 0);
 639                                 vin_dev->hw_ops->vin_wr_stream_set(vin_dev, 0);
 640                         }
 641                         line->stream_count--;
 642                 }
 643                 mutex_unlock(&line->stream_lock);
 644         }
 645
 646         if (enable)
 647                 vin_enable_output(line);
 648         else
 649                 vin_disable_output(line);
 650
 651         return 0;
 652 }
 653
 654 static struct v4l2_mbus_framefmt *
 655 __vin_get_format(struct vin_line *line,
 656                 struct v4l2_subdev_state *state,
 657                 unsigned int pad,
 658                 enum v4l2_subdev_format_whence which)
 659 {
 660         if (which == V4L2_SUBDEV_FORMAT_TRY)
 661                 return v4l2_subdev_get_try_format(&line->subdev, state, pad);
 662         return &line->fmt[pad];
 663 }
 664
 665 static void vin_try_format(struct vin_line *line,
 666                                 struct v4l2_subdev_state *state,
 667                                 unsigned int pad,
 668                                 struct v4l2_mbus_framefmt *fmt,
 669                                 enum v4l2_subdev_format_whence which)
 670 {
 671         unsigned int i;
 672
 673         switch (pad) {
 674         case STF_VIN_PAD_SINK:
 675                 /* Set format on sink pad */
 676
 677                 for (i = 0; i < line->nformats; i++)
 678                         if (fmt->code == line->formats[i].code)
 679                                 break;
 680
 681                 /* If not found, use UYVY as default */
 682                 if (i >= line->nformats)
 683                         fmt->code = line->formats[0].code;
 684
 685                 fmt->width = clamp_t(u32,
 686                                 fmt->width, STFCAMSS_FRAME_MIN_WIDTH, STFCAMSS_FRAME_MAX_WIDTH);
 687                 fmt->height = clamp_t(u32,
 688                                 fmt->height, STFCAMSS_FRAME_MIN_HEIGHT, STFCAMSS_FRAME_MAX_HEIGHT);
 689
 690                 fmt->field = V4L2_FIELD_NONE;
 691                 fmt->colorspace = V4L2_COLORSPACE_SRGB;
 692                 fmt->flags = 0;
 693
 694                 break;
 695
 696         case STF_VIN_PAD_SRC:
 697                 /* Set and return a format same as sink pad */
 698                 *fmt = *__vin_get_format(line, state, STF_VIN_PAD_SINK, which);
 699                 break;
 700         }
 701
 702         fmt->colorspace = V4L2_COLORSPACE_SRGB;
 703 }
 704
 705 static int vin_enum_mbus_code(struct v4l2_subdev *sd,
 706                                 struct v4l2_subdev_state *state,
 707                                 struct v4l2_subdev_mbus_code_enum *code)
 708 {
 709         struct vin_line *line = v4l2_get_subdevdata(sd);
 710
 711         if (code->index >= line->nformats)
 712                 return -EINVAL;
 713         if (code->pad == STF_VIN_PAD_SINK) {
 714                 code->code = line->formats[code->index].code;
 715         } else {
 716                 struct v4l2_mbus_framefmt *sink_fmt;
 717
 718                 sink_fmt = __vin_get_format(line, state, STF_VIN_PAD_SINK,
 719                                         code->which);
 720
 721                 code->code = sink_fmt->code;
 722                 if (!code->code)
 723                         return -EINVAL;
 724         }
 725         code->flags = 0;
 726
 727         return 0;
 728 }
 729
 730 static int vin_enum_frame_size(struct v4l2_subdev *sd,
 731                                 struct v4l2_subdev_state *state,
 732                                 struct v4l2_subdev_frame_size_enum *fse)
 733 {
 734         struct vin_line *line = v4l2_get_subdevdata(sd);
 735         struct v4l2_mbus_framefmt format;
 736
 737         if (fse->index != 0)
 738                 return -EINVAL;
 739
 740         format.code = fse->code;
 741         format.width = 1;
 742         format.height = 1;
 743         vin_try_format(line, state, fse->pad, &format, fse->which);
 744         fse->min_width = format.width;
 745         fse->min_height = format.height;
 746
 747         if (format.code != fse->code)
 748                 return -EINVAL;
 749
 750         format.code = fse->code;
 751         format.width = -1;
 752         format.height = -1;
 753         vin_try_format(line, state, fse->pad, &format, fse->which);
 754         fse->max_width = format.width;
 755         fse->max_height = format.height;
 756
 757         return 0;
 758 }
 759
 760 static int vin_get_format(struct v4l2_subdev *sd,
 761                         struct v4l2_subdev_state *state,
 762                         struct v4l2_subdev_format *fmt)
 763 {
 764         struct vin_line *line = v4l2_get_subdevdata(sd);
 765         struct v4l2_mbus_framefmt *format;
 766
 767         format = __vin_get_format(line, state, fmt->pad, fmt->which);
 768         if (format == NULL)
 769                 return -EINVAL;
 770
 771         fmt->format = *format;
 772
 773         return 0;
 774 }
 775
 776 static int vin_set_format(struct v4l2_subdev *sd,
 777                         struct v4l2_subdev_state *state,
 778                         struct v4l2_subdev_format *fmt)
 779 {
 780         struct vin_line *line = v4l2_get_subdevdata(sd);
 781         struct v4l2_mbus_framefmt *format;
 782
 783         format = __vin_get_format(line, state, fmt->pad, fmt->which);
 784         if (format == NULL)
 785                 return -EINVAL;
 786
 787         mutex_lock(&line->stream_lock);
 788         if (line->stream_count) {
 789                 fmt->format = *format;
 790                 mutex_unlock(&line->stream_lock);
 791                 goto out;
 792         } else {
 793                 vin_try_format(line, state, fmt->pad, &fmt->format, fmt->which);
 794                 *format = fmt->format;
 795         }
 796         mutex_unlock(&line->stream_lock);
 797
 798         if (fmt->pad == STF_VIN_PAD_SINK) {
 799                 /* Propagate the format from sink to source */
 800                 format = __vin_get_format(line, state, STF_VIN_PAD_SRC,
 801                                         fmt->which);
 802
 803                 *format = fmt->format;
 804                 vin_try_format(line, state, STF_VIN_PAD_SRC, format,
 805                                         fmt->which);
 806         }
 807
 808 out:
 809         return 0;
 810 }
 811
 812 static int vin_init_formats(struct v4l2_subdev *sd,
 813                         struct v4l2_subdev_fh *fh)
 814 {
 815         struct v4l2_subdev_format format = {
 816                 .pad = STF_VIN_PAD_SINK,
 817                 .which = fh ? V4L2_SUBDEV_FORMAT_TRY :
 818                                 V4L2_SUBDEV_FORMAT_ACTIVE,
 819                 .format = {
 820                         .code = MEDIA_BUS_FMT_RGB565_2X8_LE,
 821                         .width = 1920,
 822                         .height = 1080
 823                 }
 824         };
 825
 826         return vin_set_format(sd, fh ? fh->state : NULL, &format);
 827 }
 828
 829 static void vin_output_init_addrs(struct vin_line *line)
 830 {
 831         struct vin_output *output = &line->output;
 832         struct stf_vin2_dev *vin_dev = line_to_vin2_dev(line);
 833         dma_addr_t ping_addr;
 834         dma_addr_t pong_addr;
 835         dma_addr_t y_addr, uv_addr;
 836
 837         output->active_buf = 0;
 838
 839         if (output->buf[0]) {
 840                 ping_addr = output->buf[0]->addr[0];
 841                 y_addr = output->buf[0]->addr[0];
 842                 uv_addr = output->buf[0]->addr[1];
 843         } else
 844                 return;
 845
 846         if (output->buf[1])
 847                 pong_addr = output->buf[1]->addr[0];
 848         else
 849                 pong_addr = ping_addr;
 850
 851         switch (stf_vin_map_isp_line(line->id)) {
 852         case STF_ISP_LINE_SRC:
 853                 vin_dev->hw_ops->vin_isp_set_yuv_addr(vin_dev,
 854                         y_addr, uv_addr);
 855                 break;
 856         case STF_ISP_LINE_SRC_SS0:
 857                 vin_dev->hw_ops->vin_isp_set_ss0_addr(vin_dev,
 858                         y_addr, uv_addr);
 859                 break;
 860         case STF_ISP_LINE_SRC_SS1:
 861                 vin_dev->hw_ops->vin_isp_set_ss1_addr(vin_dev,
 862                         y_addr, uv_addr);
 863                 break;
 864         case STF_ISP_LINE_SRC_ITIW:
 865                 vin_dev->hw_ops->vin_isp_set_itiw_addr(vin_dev,
 866                         y_addr, uv_addr);
 867                 break;
 868         case STF_ISP_LINE_SRC_ITIR:
 869                 vin_dev->hw_ops->vin_isp_set_itir_addr(vin_dev,
 870                         y_addr, uv_addr);
 871                 break;
 872         case STF_ISP_LINE_SRC_RAW:
 873                 vin_dev->hw_ops->vin_isp_set_raw_addr(vin_dev, y_addr);
 874                 break;
 875         case STF_ISP_LINE_SRC_SCD_Y:
 876                 output->frame_skip = ISP_AWB_OECF_SKIP_FRAME;
 877                 vin_dev->hw_ops->vin_isp_set_scd_addr(vin_dev,
 878                         y_addr, uv_addr, AWB_TYPE);
 879                 break;
 880         default:
 881                 if (line->id == VIN_LINE_WR) {
 882                         vin_dev->hw_ops->vin_wr_set_ping_addr(vin_dev, ping_addr);
 883 #ifdef VIN_TWO_BUFFER
 884                         vin_dev->hw_ops->vin_wr_set_pong_addr(vin_dev, pong_addr);
 885 #else
 886                         vin_dev->hw_ops->vin_wr_set_pong_addr(vin_dev, ping_addr);
 887 #endif
 888                 }
 889                 break;
 890         }
 891 }
 892
 893 static void vin_init_outputs(struct vin_line *line)
 894 {
 895         struct vin_output *output = &line->output;
 896
 897         output->state = VIN_OUTPUT_OFF;
 898         output->buf[0] = NULL;
 899         output->buf[1] = NULL;
 900         output->active_buf = 0;
 901         INIT_LIST_HEAD(&output->pending_bufs);
 902         INIT_LIST_HEAD(&output->ready_bufs);
 903 }
 904
 905 static void vin_buf_add_ready(struct vin_output *output,
 906                                 struct stfcamss_buffer *buffer)
 907 {
 908         INIT_LIST_HEAD(&buffer->queue);
 909         list_add_tail(&buffer->queue, &output->ready_bufs);
 910 }
 911
 912 static struct stfcamss_buffer *vin_buf_get_ready(struct vin_output *output)
 913 {
 914         struct stfcamss_buffer *buffer = NULL;
 915
 916         if (!list_empty(&output->ready_bufs)) {
 917                 buffer = list_first_entry(&output->ready_bufs,
 918                                         struct stfcamss_buffer,
 919                                         queue);
 920                 list_del(&buffer->queue);
 921         }
 922
 923         return buffer;
 924 }
 925
 926 static void vin_buf_add_pending(struct vin_output *output,
 927                                 struct stfcamss_buffer *buffer)
 928 {
 929         INIT_LIST_HEAD(&buffer->queue);
 930         list_add_tail(&buffer->queue, &output->pending_bufs);
 931 }
 932
 933 static struct stfcamss_buffer *vin_buf_get_pending(struct vin_output *output)
 934 {
 935         struct stfcamss_buffer *buffer = NULL;
 936
 937         if (!list_empty(&output->pending_bufs)) {
 938                 buffer = list_first_entry(&output->pending_bufs,
 939                                         struct stfcamss_buffer,
 940                                         queue);
 941                 list_del(&buffer->queue);
 942         }
 943
 944         return buffer;
 945 }
 946
 947 #ifdef UNUSED_CODE
 948 static void vin_output_checkpending(struct vin_line *line)
 949 {
 950         struct vin_output *output = &line->output;
 951
 952         if (output->state == VIN_OUTPUT_STOPPING) {
 953                 /* Release last buffer when hw is idle */
 954                 if (output->last_buffer) {
 955                         // vb2_buffer_done(&output->last_buffer->vb.vb2_buf,
 956                         //              VB2_BUF_STATE_DONE);
 957                         vin_buf_add_pending(output, output->last_buffer);
 958                         output->last_buffer = NULL;
 959                 }
 960                 output->state = VIN_OUTPUT_IDLE;
 961
 962                 /* Buffers received in stopping state are queued in */
 963                 /* dma pending queue, start next capture here */
 964                 output->buf[0] = vin_buf_get_pending(output);
 965 #ifdef VIN_TWO_BUFFER
 966                 if (line->id == VIN_LINE_WR)
 967                         output->buf[1] = vin_buf_get_pending(output);
 968 #endif
 969
 970                 if (!output->buf[0] && output->buf[1]) {
 971                         output->buf[0] = output->buf[1];
 972                         output->buf[1] = NULL;
 973                 }
 974
 975                 if (output->buf[0])
 976                         output->state = VIN_OUTPUT_SINGLE;
 977
 978 #ifdef VIN_TWO_BUFFER
 979                 if (output->buf[1] && line->id == VIN_LINE_WR)
 980                         output->state = VIN_OUTPUT_CONTINUOUS;
 981 #endif
 982                 vin_output_init_addrs(line);
 983         }
 984 }
 985 #endif
 986
 987 static void vin_buf_update_on_last(struct vin_line *line)
 988 {
 989         struct vin_output *output = &line->output;
 990
 991         switch (output->state) {
 992         case VIN_OUTPUT_CONTINUOUS:
 993                 output->state = VIN_OUTPUT_SINGLE;
 994                 output->active_buf = !output->active_buf;
 995                 break;
 996         case VIN_OUTPUT_SINGLE:
 997                 output->state = VIN_OUTPUT_STOPPING;
 998                 break;
 999         default:
1000                 st_err_ratelimited(ST_VIN,
1001                                 "Last buff in wrong state! %d\n",
1002                                 output->state);
1003                 break;
1004         }
1005 }
1006
1007 static void vin_buf_update_on_next(struct vin_line *line)
1008 {
1009         struct vin_output *output = &line->output;
1010
1011         switch (output->state) {
1012         case VIN_OUTPUT_CONTINUOUS:
1013                 output->active_buf = !output->active_buf;
1014                 break;
1015         case VIN_OUTPUT_SINGLE:
1016         default:
1017 #ifdef VIN_TWO_BUFFER
1018                 if (line->id == VIN_LINE_WR)
1019                         st_err_ratelimited(ST_VIN,
1020                                 "Next buf in wrong state! %d\n",
1021                                 output->state);
1022 #endif
1023                 break;
1024         }
1025 }
1026
1027 static void vin_buf_update_on_new(struct vin_line *line,
1028                                 struct vin_output *output,
1029                                 struct stfcamss_buffer *new_buf)
1030 {
1031 #ifdef VIN_TWO_BUFFER
1032         struct stf_vin2_dev *vin_dev = line_to_vin2_dev(line);
1033         int inactive_idx;
1034 #endif
1035
1036         switch (output->state) {
1037         case VIN_OUTPUT_SINGLE:
1038 #ifdef VIN_TWO_BUFFER
1039                 int inactive_idx = !output->active_buf;
1040
1041                 if (!output->buf[inactive_idx] && line->id == VIN_LINE_WR) {
1042                         output->buf[inactive_idx] = new_buf;
1043                         if (inactive_idx)
1044                                 vin_dev->hw_ops->vin_wr_set_pong_addr(vin_dev,
1045                                                 output->buf[1]->addr[0]);
1046                         else
1047                                 vin_dev->hw_ops->vin_wr_set_ping_addr(vin_dev,
1048                                                 output->buf[0]->addr[0]);
1049                         output->state = VIN_OUTPUT_CONTINUOUS;
1050
1051                 } else {
1052                         vin_buf_add_pending(output, new_buf);
1053                         if (line->id == VIN_LINE_WR)
1054                                 st_warn(ST_VIN, "Inactive buffer is busy\n");
1055                 }
1056 #else
1057                 vin_buf_add_pending(output, new_buf);
1058 #endif
1059                 break;
1060         case VIN_OUTPUT_IDLE:
1061                 st_warn(ST_VIN, "Output idle buffer set!\n");
1062                 if (!output->buf[0]) {
1063                         output->buf[0] = new_buf;
1064                         vin_output_init_addrs(line);
1065                         output->state = VIN_OUTPUT_SINGLE;
1066                 } else {
1067                         vin_buf_add_pending(output, new_buf);
1068                         st_warn(ST_VIN, "Output idle with buffer set!\n");
1069                 }
1070                 break;
1071         case VIN_OUTPUT_STOPPING:
1072                 if (output->last_buffer) {
1073                         output->buf[output->active_buf] = output->last_buffer;
1074                         output->last_buffer = NULL;
1075                 } else
1076                         st_err(ST_VIN,  "stop state lost lastbuffer!\n");
1077                 output->state = VIN_OUTPUT_SINGLE;
1078                 // vin_output_checkpending(line);
1079                 vin_buf_add_pending(output, new_buf);
1080                 break;
1081         case VIN_OUTPUT_CONTINUOUS:
1082         default:
1083                 vin_buf_add_pending(output, new_buf);
1084                 break;
1085         }
1086 }
1087
1088 static void vin_buf_flush(struct vin_output *output,
1089                                 enum vb2_buffer_state state)
1090 {
1091         struct stfcamss_buffer *buf;
1092         struct stfcamss_buffer *t;
1093
1094         list_for_each_entry_safe(buf, t, &output->pending_bufs, queue) {
1095                 vb2_buffer_done(&buf->vb.vb2_buf, state);
1096                 list_del(&buf->queue);
1097         }
1098         list_for_each_entry_safe(buf, t, &output->ready_bufs, queue) {
1099                 vb2_buffer_done(&buf->vb.vb2_buf, state);
1100                 list_del(&buf->queue);
1101         }
1102 }
1103
1104 extern void sifive_l2_flush64_range(unsigned long start, unsigned long len);
1105 static void vin_buffer_done(struct vin_line *line, struct vin_params *params)
1106 {
1107         struct stfcamss_buffer *ready_buf;
1108         struct vin_output *output = &line->output;
1109         unsigned long flags;
1110         u64 ts = ktime_get_ns();
1111
1112         if (output->state == VIN_OUTPUT_OFF
1113                 || output->state == VIN_OUTPUT_RESERVED)
1114                 return;
1115
1116         spin_lock_irqsave(&line->output_lock, flags);
1117
1118         while ((ready_buf = vin_buf_get_ready(output))) {
1119                 ready_buf->vb.vb2_buf.timestamp = ts;
1120                 ready_buf->vb.sequence = output->sequence++;
1121
1122                 /* The stf_isp_ctrl currently buffered with mmap,
1123                  * which will not update cache by default.
1124                  * Flush L2 cache to make sure data is updated.
1125                  */
1126                 if (ready_buf->vb.vb2_buf.memory == VB2_MEMORY_MMAP)
1127                         sifive_l2_flush64_range(ready_buf->addr[0], ready_buf->sizeimage);
1128
1129                 vb2_buffer_done(&ready_buf->vb.vb2_buf, VB2_BUF_STATE_DONE);
1130         }
1131
1132         spin_unlock_irqrestore(&line->output_lock, flags);
1133 }
1134
1135 static void vin_change_buffer(struct vin_line *line)
1136 {
1137         struct stfcamss_buffer *ready_buf;
1138         struct vin_output *output = &line->output;
1139         struct stf_vin2_dev *vin_dev = line_to_vin2_dev(line);
1140         dma_addr_t *new_addr;
1141         unsigned long flags;
1142         u32 active_index;
1143         int scd_type;
1144
1145         if (output->state == VIN_OUTPUT_OFF
1146                 || output->state == VIN_OUTPUT_STOPPING
1147                 || output->state == VIN_OUTPUT_RESERVED
1148                 || output->state == VIN_OUTPUT_IDLE) {
1149                 st_err_ratelimited(ST_VIN,
1150                                 "%s: output state no ready %d!, %d\n",
1151                                 __func__, output->state, line->id);
1152                 return;
1153         }
1154
1155         spin_lock_irqsave(&line->output_lock, flags);
1156
1157         active_index = output->active_buf;
1158
1159         ready_buf = output->buf[active_index];
1160         if (!ready_buf) {
1161                 st_err_ratelimited(ST_VIN,
1162                                         "Missing ready buf %d %d!\n",
1163                                         active_index, output->state);
1164                 active_index = !active_index;
1165                 ready_buf = output->buf[active_index];
1166                 if (!ready_buf) {
1167                         st_err_ratelimited(ST_VIN,
1168                                         "Missing ready buf 2 %d %d!\n",
1169                                         active_index, output->state);
1170                         goto out_unlock;
1171                 }
1172         }
1173
1174         /* Get next buffer */
1175         output->buf[active_index] = vin_buf_get_pending(output);
1176         if (!output->buf[active_index]) {
1177                 /* No next buffer - set same address */
1178                 new_addr = ready_buf->addr;
1179                 vin_buf_update_on_last(line);
1180         } else {
1181                 new_addr = output->buf[active_index]->addr;
1182                 vin_buf_update_on_next(line);
1183         }
1184
1185         if (output->state == VIN_OUTPUT_STOPPING)
1186                 output->last_buffer = ready_buf;
1187         else {
1188                 switch (stf_vin_map_isp_line(line->id)) {
1189                 case STF_ISP_LINE_SRC:
1190                         vin_dev->hw_ops->vin_isp_set_yuv_addr(vin_dev,
1191                                 new_addr[0], new_addr[1]);
1192                         break;
1193                 case STF_ISP_LINE_SRC_SS0:
1194                         vin_dev->hw_ops->vin_isp_set_ss0_addr(vin_dev,
1195                                 new_addr[0], new_addr[1]);
1196                         break;
1197                 case STF_ISP_LINE_SRC_SS1:
1198                         vin_dev->hw_ops->vin_isp_set_ss1_addr(vin_dev,
1199                                 new_addr[0], new_addr[1]);
1200                         break;
1201                 case STF_ISP_LINE_SRC_ITIW:
1202                         vin_dev->hw_ops->vin_isp_set_itiw_addr(vin_dev,
1203                                 new_addr[0], new_addr[1]);
1204                         break;
1205                 case STF_ISP_LINE_SRC_ITIR:
1206                         vin_dev->hw_ops->vin_isp_set_itir_addr(vin_dev,
1207                                 new_addr[0], new_addr[1]);
1208                         break;
1209                 case STF_ISP_LINE_SRC_RAW:
1210                         vin_dev->hw_ops->vin_isp_set_raw_addr(vin_dev, new_addr[0]);
1211                         break;
1212                 case STF_ISP_LINE_SRC_SCD_Y:
1213                         scd_type = vin_dev->hw_ops->vin_isp_get_scd_type(vin_dev);
1214                         ready_buf->vb.flags &= ~(V4L2_BUF_FLAG_PFRAME | V4L2_BUF_FLAG_BFRAME);
1215                         if (scd_type == AWB_TYPE)
1216                                 ready_buf->vb.flags |= V4L2_BUF_FLAG_PFRAME;
1217                         else
1218                                 ready_buf->vb.flags |= V4L2_BUF_FLAG_BFRAME;
1219                         if (!output->frame_skip) {
1220                                 output->frame_skip = ISP_AWB_OECF_SKIP_FRAME;
1221                                 scd_type = scd_type == AWB_TYPE ? OECF_TYPE : AWB_TYPE;
1222                         } else {
1223                                 output->frame_skip--;
1224                                 scd_type = scd_type == AWB_TYPE ? AWB_TYPE : OECF_TYPE;
1225                         }
1226                         vin_dev->hw_ops->vin_isp_set_scd_addr(vin_dev,
1227                                 new_addr[0], new_addr[1], scd_type);
1228                         break;
1229                 default:
1230                         if (line->id == VIN_LINE_WR) {
1231 #ifdef VIN_TWO_BUFFER
1232                                 if (active_index)
1233                                         vin_dev->hw_ops->vin_wr_set_pong_addr(vin_dev,
1234                                                         new_addr[0]);
1235                                 else
1236                                         vin_dev->hw_ops->vin_wr_set_ping_addr(vin_dev,
1237                                                         new_addr[0]);
1238 #else
1239                                 vin_dev->hw_ops->vin_wr_set_ping_addr(vin_dev,
1240                                                         new_addr[0]);
1241                                 vin_dev->hw_ops->vin_wr_set_pong_addr(vin_dev,
1242                                                         new_addr[0]);
1243 #endif
1244                         }
1245                         break;
1246                 }
1247
1248                 vin_buf_add_ready(output, ready_buf);
1249         }
1250
1251         spin_unlock_irqrestore(&line->output_lock, flags);
1252         return;
1253
1254 out_unlock:
1255         spin_unlock_irqrestore(&line->output_lock, flags);
1256 }
1257
1258 static int vin_queue_buffer(struct stfcamss_video *vid,
1259                                 struct stfcamss_buffer *buf)
1260 {
1261         struct vin_line *line = container_of(vid, struct vin_line, video_out);
1262         struct vin_output *output;
1263         unsigned long flags;
1264
1265
1266         output = &line->output;
1267
1268         spin_lock_irqsave(&line->output_lock, flags);
1269
1270         vin_buf_update_on_new(line, output, buf);
1271
1272         spin_unlock_irqrestore(&line->output_lock, flags);
1273
1274         return 0;
1275 }
1276
1277 static int vin_flush_buffers(struct stfcamss_video *vid,
1278                                 enum vb2_buffer_state state)
1279 {
1280         struct vin_line *line = container_of(vid, struct vin_line, video_out);
1281         struct vin_output *output = &line->output;
1282         unsigned long flags;
1283
1284         spin_lock_irqsave(&line->output_lock, flags);
1285
1286         vin_buf_flush(output, state);
1287         if (output->buf[0])
1288                 vb2_buffer_done(&output->buf[0]->vb.vb2_buf, state);
1289
1290         if (output->buf[1])
1291                 vb2_buffer_done(&output->buf[1]->vb.vb2_buf, state);
1292
1293         if (output->last_buffer) {
1294                 vb2_buffer_done(&output->last_buffer->vb.vb2_buf, state);
1295                 output->last_buffer = NULL;
1296         }
1297         output->buf[0] = output->buf[1] = NULL;
1298
1299         spin_unlock_irqrestore(&line->output_lock, flags);
1300         return 0;
1301 }
1302
1303 static int vin_link_setup(struct media_entity *entity,
1304                         const struct media_pad *local,
1305                         const struct media_pad *remote, u32 flags)
1306 {
1307         if (flags & MEDIA_LNK_FL_ENABLED)
1308                 if (media_entity_remote_pad(local))
1309                         return -EBUSY;
1310         return 0;
1311 }
1312
1313 static const struct v4l2_subdev_core_ops vin_core_ops = {
1314         .s_power = vin_set_power,
1315 };
1316
1317 static const struct v4l2_subdev_video_ops vin_video_ops = {
1318         .s_stream = vin_set_stream,
1319 };
1320
1321 static const struct v4l2_subdev_pad_ops vin_pad_ops = {
1322         .enum_mbus_code   = vin_enum_mbus_code,
1323         .enum_frame_size  = vin_enum_frame_size,
1324         .get_fmt          = vin_get_format,
1325         .set_fmt          = vin_set_format,
1326 };
1327
1328 static const struct v4l2_subdev_ops vin_v4l2_ops = {
1329         .core = &vin_core_ops,
1330         .video = &vin_video_ops,
1331         .pad = &vin_pad_ops,
1332 };
1333
1334 static const struct v4l2_subdev_internal_ops vin_v4l2_internal_ops = {
1335         .open = vin_init_formats,
1336 };
1337
1338 static const struct stfcamss_video_ops stfcamss_vin_video_ops = {
1339         .queue_buffer = vin_queue_buffer,
1340         .flush_buffers = vin_flush_buffers,
1341 };
1342
1343 static const struct media_entity_operations vin_media_ops = {
1344         .link_setup = vin_link_setup,
1345         .link_validate = v4l2_subdev_link_validate,
1346 };
1347
1348 int stf_vin_register(struct stf_vin2_dev *vin_dev, struct v4l2_device *v4l2_dev)
1349 {
1350         struct v4l2_subdev *sd;
1351         struct stfcamss_video *video_out;
1352         struct media_pad *pads;
1353         int ret;
1354         int i;
1355
1356         for (i = 0; i < STF_ISP_LINE_MAX + 1; i++) {
1357                 char name[32];
1358                 char *sub_name = get_line_subdevname(i);
1359                 int is_mp;
1360
1361                 is_mp = (stf_vin_map_isp_line(i) == STF_ISP_LINE_SRC) ? true : false;
1362                 is_mp = false;
1363                 sd = &vin_dev->line[i].subdev;
1364                 pads = vin_dev->line[i].pads;
1365                 video_out = &vin_dev->line[i].video_out;
1366                 video_out->id = i;
1367
1368                 v4l2_subdev_init(sd, &vin_v4l2_ops);
1369                 sd->internal_ops = &vin_v4l2_internal_ops;
1370                 sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
1371                 snprintf(sd->name, ARRAY_SIZE(sd->name), "%s%d_%s",
1372                         STF_VIN_NAME, 0, sub_name);
1373                 v4l2_set_subdevdata(sd, &vin_dev->line[i]);
1374
1375                 ret = vin_init_formats(sd, NULL);
1376                 if (ret < 0) {
1377                         st_err(ST_VIN, "Failed to init format: %d\n", ret);
1378                         goto err_init;
1379                 }
1380
1381                 pads[STF_VIN_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
1382                 pads[STF_VIN_PAD_SRC].flags = MEDIA_PAD_FL_SOURCE;
1383
1384                 sd->entity.function =
1385                         MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER;
1386                 sd->entity.ops = &vin_media_ops;
1387                 ret = media_entity_pads_init(&sd->entity,
1388                                 STF_VIN_PADS_NUM, pads);
1389                 if (ret < 0) {
1390                         st_err(ST_VIN, "Failed to init media entity: %d\n", ret);
1391                         goto err_init;
1392                 }
1393
1394                 ret = v4l2_device_register_subdev(v4l2_dev, sd);
1395                 if (ret < 0) {
1396                         st_err(ST_VIN, "Failed to register subdev: %d\n", ret);
1397                         goto err_reg_subdev;
1398                 }
1399
1400                 video_out->ops = &stfcamss_vin_video_ops;
1401                 video_out->bpl_alignment = 16 * 8;
1402
1403                 snprintf(name, ARRAY_SIZE(name), "%s_%s%d",
1404                         sd->name, "video", i);
1405                 ret = stf_video_register(video_out, v4l2_dev, name, is_mp);
1406                 if (ret < 0) {
1407                         st_err(ST_VIN, "Failed to register video node: %d\n",
1408                                         ret);
1409                         goto err_vid_reg;
1410                 }
1411
1412                 ret = media_create_pad_link(
1413                         &sd->entity, STF_VIN_PAD_SRC,
1414                         &video_out->vdev.entity, 0,
1415                         MEDIA_LNK_FL_IMMUTABLE | MEDIA_LNK_FL_ENABLED);
1416                 if (ret < 0) {
1417                         st_err(ST_VIN, "Failed to link %s->%s entities: %d\n",
1418                                 sd->entity.name, video_out->vdev.entity.name,
1419                                 ret);
1420                         goto err_create_link;
1421                 }
1422         }
1423
1424         return 0;
1425
1426 err_create_link:
1427         stf_video_unregister(video_out);
1428 err_vid_reg:
1429         v4l2_device_unregister_subdev(sd);
1430 err_reg_subdev:
1431         media_entity_cleanup(&sd->entity);
1432 err_init:
1433         for (i--; i >= 0; i--) {
1434                 sd = &vin_dev->line[i].subdev;
1435                 video_out = &vin_dev->line[i].video_out;
1436
1437                 stf_video_unregister(video_out);
1438                 v4l2_device_unregister_subdev(sd);
1439                 media_entity_cleanup(&sd->entity);
1440         }
1441         return ret;
1442 }
1443
1444 int stf_vin_unregister(struct stf_vin2_dev *vin_dev)
1445 {
1446         struct v4l2_subdev *sd;
1447         struct stfcamss_video *video_out;
1448         int i;
1449
1450         mutex_destroy(&vin_dev->power_lock);
1451         for (i = 0; i < STF_DUMMY_MODULE_NUMS; i++)
1452                 mutex_destroy(&vin_dev->dummy_buffer[i].stream_lock);
1453
1454         for (i = 0; i < STF_ISP_LINE_MAX + 1; i++) {
1455                 sd = &vin_dev->line[i].subdev;
1456                 video_out = &vin_dev->line[i].video_out;
1457
1458                 stf_video_unregister(video_out);
1459                 v4l2_device_unregister_subdev(sd);
1460                 media_entity_cleanup(&sd->entity);
1461                 mutex_destroy(&vin_dev->line[i].stream_lock);
1462                 mutex_destroy(&vin_dev->line[i].power_lock);
1463         }
1464         return 0;
1465 }