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         st_debug(ST_VIDEO, "%s, pad %d, fmt code  %x\n",
 784                         __func__, fmt->pad, fmt->format.code);
 785
 786         format = __vin_get_format(line, state, fmt->pad, fmt->which);
 787         if (format == NULL)
 788                 return -EINVAL;
 789
 790         mutex_lock(&line->stream_lock);
 791         if (line->stream_count) {
 792                 fmt->format = *format;
 793                 mutex_unlock(&line->stream_lock);
 794                 goto out;
 795         } else {
 796                 vin_try_format(line, state, fmt->pad, &fmt->format, fmt->which);
 797                 *format = fmt->format;
 798         }
 799         mutex_unlock(&line->stream_lock);
 800
 801         if (fmt->pad == STF_VIN_PAD_SINK) {
 802                 /* Propagate the format from sink to source */
 803                 format = __vin_get_format(line, state, STF_VIN_PAD_SRC,
 804                                         fmt->which);
 805
 806                 *format = fmt->format;
 807                 vin_try_format(line, state, STF_VIN_PAD_SRC, format,
 808                                         fmt->which);
 809         }
 810
 811 out:
 812         return 0;
 813 }
 814
 815 static int vin_init_formats(struct v4l2_subdev *sd,
 816                         struct v4l2_subdev_fh *fh)
 817 {
 818         struct v4l2_subdev_format format = {
 819                 .pad = STF_VIN_PAD_SINK,
 820                 .which = fh ? V4L2_SUBDEV_FORMAT_TRY :
 821                                 V4L2_SUBDEV_FORMAT_ACTIVE,
 822                 .format = {
 823                         .code = MEDIA_BUS_FMT_RGB565_2X8_LE,
 824                         .width = 1920,
 825                         .height = 1080
 826                 }
 827         };
 828
 829         return vin_set_format(sd, fh ? fh->state : NULL, &format);
 830 }
 831
 832 static void vin_output_init_addrs(struct vin_line *line)
 833 {
 834         struct vin_output *output = &line->output;
 835         struct stf_vin2_dev *vin_dev = line_to_vin2_dev(line);
 836         dma_addr_t ping_addr;
 837         dma_addr_t pong_addr;
 838         dma_addr_t y_addr, uv_addr;
 839
 840         output->active_buf = 0;
 841
 842         if (output->buf[0]) {
 843                 ping_addr = output->buf[0]->addr[0];
 844                 y_addr = output->buf[0]->addr[0];
 845                 uv_addr = output->buf[0]->addr[1];
 846         } else
 847                 return;
 848
 849         if (output->buf[1])
 850                 pong_addr = output->buf[1]->addr[0];
 851         else
 852                 pong_addr = ping_addr;
 853
 854         switch (stf_vin_map_isp_line(line->id)) {
 855         case STF_ISP_LINE_SRC:
 856                 vin_dev->hw_ops->vin_isp_set_yuv_addr(vin_dev,
 857                         y_addr, uv_addr);
 858                 break;
 859         case STF_ISP_LINE_SRC_SS0:
 860                 vin_dev->hw_ops->vin_isp_set_ss0_addr(vin_dev,
 861                         y_addr, uv_addr);
 862                 break;
 863         case STF_ISP_LINE_SRC_SS1:
 864                 vin_dev->hw_ops->vin_isp_set_ss1_addr(vin_dev,
 865                         y_addr, uv_addr);
 866                 break;
 867         case STF_ISP_LINE_SRC_ITIW:
 868                 vin_dev->hw_ops->vin_isp_set_itiw_addr(vin_dev,
 869                         y_addr, uv_addr);
 870                 break;
 871         case STF_ISP_LINE_SRC_ITIR:
 872                 vin_dev->hw_ops->vin_isp_set_itir_addr(vin_dev,
 873                         y_addr, uv_addr);
 874                 break;
 875         case STF_ISP_LINE_SRC_RAW:
 876                 vin_dev->hw_ops->vin_isp_set_raw_addr(vin_dev, y_addr);
 877                 break;
 878         case STF_ISP_LINE_SRC_SCD_Y:
 879                 output->frame_skip = ISP_AWB_OECF_SKIP_FRAME;
 880                 vin_dev->hw_ops->vin_isp_set_scd_addr(vin_dev,
 881                         y_addr, uv_addr, AWB_TYPE);
 882                 break;
 883         default:
 884                 if (line->id == VIN_LINE_WR) {
 885                         vin_dev->hw_ops->vin_wr_set_ping_addr(vin_dev, ping_addr);
 886 #ifdef VIN_TWO_BUFFER
 887                         vin_dev->hw_ops->vin_wr_set_pong_addr(vin_dev, pong_addr);
 888 #else
 889                         vin_dev->hw_ops->vin_wr_set_pong_addr(vin_dev, ping_addr);
 890 #endif
 891                 }
 892                 break;
 893         }
 894 }
 895
 896 static void vin_init_outputs(struct vin_line *line)
 897 {
 898         struct vin_output *output = &line->output;
 899
 900         output->state = VIN_OUTPUT_OFF;
 901         output->buf[0] = NULL;
 902         output->buf[1] = NULL;
 903         output->active_buf = 0;
 904         INIT_LIST_HEAD(&output->pending_bufs);
 905         INIT_LIST_HEAD(&output->ready_bufs);
 906 }
 907
 908 static void vin_buf_add_ready(struct vin_output *output,
 909                                 struct stfcamss_buffer *buffer)
 910 {
 911         INIT_LIST_HEAD(&buffer->queue);
 912         list_add_tail(&buffer->queue, &output->ready_bufs);
 913 }
 914
 915 static struct stfcamss_buffer *vin_buf_get_ready(struct vin_output *output)
 916 {
 917         struct stfcamss_buffer *buffer = NULL;
 918
 919         if (!list_empty(&output->ready_bufs)) {
 920                 buffer = list_first_entry(&output->ready_bufs,
 921                                         struct stfcamss_buffer,
 922                                         queue);
 923                 list_del(&buffer->queue);
 924         }
 925
 926         return buffer;
 927 }
 928
 929 static void vin_buf_add_pending(struct vin_output *output,
 930                                 struct stfcamss_buffer *buffer)
 931 {
 932         INIT_LIST_HEAD(&buffer->queue);
 933         list_add_tail(&buffer->queue, &output->pending_bufs);
 934 }
 935
 936 static struct stfcamss_buffer *vin_buf_get_pending(struct vin_output *output)
 937 {
 938         struct stfcamss_buffer *buffer = NULL;
 939
 940         if (!list_empty(&output->pending_bufs)) {
 941                 buffer = list_first_entry(&output->pending_bufs,
 942                                         struct stfcamss_buffer,
 943                                         queue);
 944                 list_del(&buffer->queue);
 945         }
 946
 947         return buffer;
 948 }
 949
 950 #ifdef UNUSED_CODE
 951 static void vin_output_checkpending(struct vin_line *line)
 952 {
 953         struct vin_output *output = &line->output;
 954
 955         if (output->state == VIN_OUTPUT_STOPPING) {
 956                 /* Release last buffer when hw is idle */
 957                 if (output->last_buffer) {
 958                         // vb2_buffer_done(&output->last_buffer->vb.vb2_buf,
 959                         //              VB2_BUF_STATE_DONE);
 960                         vin_buf_add_pending(output, output->last_buffer);
 961                         output->last_buffer = NULL;
 962                 }
 963                 output->state = VIN_OUTPUT_IDLE;
 964
 965                 /* Buffers received in stopping state are queued in */
 966                 /* dma pending queue, start next capture here */
 967                 output->buf[0] = vin_buf_get_pending(output);
 968 #ifdef VIN_TWO_BUFFER
 969                 if (line->id == VIN_LINE_WR)
 970                         output->buf[1] = vin_buf_get_pending(output);
 971 #endif
 972
 973                 if (!output->buf[0] && output->buf[1]) {
 974                         output->buf[0] = output->buf[1];
 975                         output->buf[1] = NULL;
 976                 }
 977
 978                 if (output->buf[0])
 979                         output->state = VIN_OUTPUT_SINGLE;
 980
 981 #ifdef VIN_TWO_BUFFER
 982                 if (output->buf[1] && line->id == VIN_LINE_WR)
 983                         output->state = VIN_OUTPUT_CONTINUOUS;
 984 #endif
 985                 vin_output_init_addrs(line);
 986         }
 987 }
 988 #endif
 989
 990 static void vin_buf_update_on_last(struct vin_line *line)
 991 {
 992         struct vin_output *output = &line->output;
 993
 994         switch (output->state) {
 995         case VIN_OUTPUT_CONTINUOUS:
 996                 output->state = VIN_OUTPUT_SINGLE;
 997                 output->active_buf = !output->active_buf;
 998                 break;
 999         case VIN_OUTPUT_SINGLE:
1000                 output->state = VIN_OUTPUT_STOPPING;
1001                 break;
1002         default:
1003                 st_err_ratelimited(ST_VIN,
1004                                 "Last buff in wrong state! %d\n",
1005                                 output->state);
1006                 break;
1007         }
1008 }
1009
1010 static void vin_buf_update_on_next(struct vin_line *line)
1011 {
1012         struct vin_output *output = &line->output;
1013
1014         switch (output->state) {
1015         case VIN_OUTPUT_CONTINUOUS:
1016                 output->active_buf = !output->active_buf;
1017                 break;
1018         case VIN_OUTPUT_SINGLE:
1019         default:
1020 #ifdef VIN_TWO_BUFFER
1021                 if (line->id == VIN_LINE_WR)
1022                         st_err_ratelimited(ST_VIN,
1023                                 "Next buf in wrong state! %d\n",
1024                                 output->state);
1025 #endif
1026                 break;
1027         }
1028 }
1029
1030 static void vin_buf_update_on_new(struct vin_line *line,
1031                                 struct vin_output *output,
1032                                 struct stfcamss_buffer *new_buf)
1033 {
1034 #ifdef VIN_TWO_BUFFER
1035         struct stf_vin2_dev *vin_dev = line_to_vin2_dev(line);
1036         int inactive_idx;
1037 #endif
1038
1039         switch (output->state) {
1040         case VIN_OUTPUT_SINGLE:
1041 #ifdef VIN_TWO_BUFFER
1042                 int inactive_idx = !output->active_buf;
1043
1044                 if (!output->buf[inactive_idx] && line->id == VIN_LINE_WR) {
1045                         output->buf[inactive_idx] = new_buf;
1046                         if (inactive_idx)
1047                                 vin_dev->hw_ops->vin_wr_set_pong_addr(vin_dev,
1048                                                 output->buf[1]->addr[0]);
1049                         else
1050                                 vin_dev->hw_ops->vin_wr_set_ping_addr(vin_dev,
1051                                                 output->buf[0]->addr[0]);
1052                         output->state = VIN_OUTPUT_CONTINUOUS;
1053
1054                 } else {
1055                         vin_buf_add_pending(output, new_buf);
1056                         if (line->id == VIN_LINE_WR)
1057                                 st_warn(ST_VIN, "Inactive buffer is busy\n");
1058                 }
1059 #else
1060                 vin_buf_add_pending(output, new_buf);
1061 #endif
1062                 break;
1063         case VIN_OUTPUT_IDLE:
1064                 st_warn(ST_VIN, "Output idle buffer set!\n");
1065                 if (!output->buf[0]) {
1066                         output->buf[0] = new_buf;
1067                         vin_output_init_addrs(line);
1068                         output->state = VIN_OUTPUT_SINGLE;
1069                 } else {
1070                         vin_buf_add_pending(output, new_buf);
1071                         st_warn(ST_VIN, "Output idle with buffer set!\n");
1072                 }
1073                 break;
1074         case VIN_OUTPUT_STOPPING:
1075                 if (output->last_buffer) {
1076                         output->buf[output->active_buf] = output->last_buffer;
1077                         output->last_buffer = NULL;
1078                 } else
1079                         st_err(ST_VIN,  "stop state lost lastbuffer!\n");
1080                 output->state = VIN_OUTPUT_SINGLE;
1081                 // vin_output_checkpending(line);
1082                 vin_buf_add_pending(output, new_buf);
1083                 break;
1084         case VIN_OUTPUT_CONTINUOUS:
1085         default:
1086                 vin_buf_add_pending(output, new_buf);
1087                 break;
1088         }
1089 }
1090
1091 static void vin_buf_flush(struct vin_output *output,
1092                                 enum vb2_buffer_state state)
1093 {
1094         struct stfcamss_buffer *buf;
1095         struct stfcamss_buffer *t;
1096
1097         list_for_each_entry_safe(buf, t, &output->pending_bufs, queue) {
1098                 vb2_buffer_done(&buf->vb.vb2_buf, state);
1099                 list_del(&buf->queue);
1100         }
1101         list_for_each_entry_safe(buf, t, &output->ready_bufs, queue) {
1102                 vb2_buffer_done(&buf->vb.vb2_buf, state);
1103                 list_del(&buf->queue);
1104         }
1105 }
1106
1107 extern void sifive_l2_flush64_range(unsigned long start, unsigned long len);
1108 static void vin_buffer_done(struct vin_line *line, struct vin_params *params)
1109 {
1110         struct stfcamss_buffer *ready_buf;
1111         struct vin_output *output = &line->output;
1112         unsigned long flags;
1113         u64 ts = ktime_get_ns();
1114
1115         if (output->state == VIN_OUTPUT_OFF
1116                 || output->state == VIN_OUTPUT_RESERVED)
1117                 return;
1118
1119         spin_lock_irqsave(&line->output_lock, flags);
1120
1121         while ((ready_buf = vin_buf_get_ready(output))) {
1122                 ready_buf->vb.vb2_buf.timestamp = ts;
1123                 ready_buf->vb.sequence = output->sequence++;
1124
1125                 /* The stf_isp_ctrl currently buffered with mmap,
1126                  * which will not update cache by default.
1127                  * Flush L2 cache to make sure data is updated.
1128                  */
1129                 if (ready_buf->vb.vb2_buf.memory == VB2_MEMORY_MMAP)
1130                         sifive_l2_flush64_range(ready_buf->addr[0], ready_buf->sizeimage);
1131
1132                 vb2_buffer_done(&ready_buf->vb.vb2_buf, VB2_BUF_STATE_DONE);
1133         }
1134
1135         spin_unlock_irqrestore(&line->output_lock, flags);
1136 }
1137
1138 static void vin_change_buffer(struct vin_line *line)
1139 {
1140         struct stfcamss_buffer *ready_buf;
1141         struct vin_output *output = &line->output;
1142         struct stf_vin2_dev *vin_dev = line_to_vin2_dev(line);
1143         dma_addr_t *new_addr;
1144         unsigned long flags;
1145         u32 active_index;
1146         int scd_type;
1147
1148         if (output->state == VIN_OUTPUT_OFF
1149                 || output->state == VIN_OUTPUT_STOPPING
1150                 || output->state == VIN_OUTPUT_RESERVED
1151                 || output->state == VIN_OUTPUT_IDLE) {
1152                 st_err_ratelimited(ST_VIN,
1153                                 "%s: output state no ready %d!, %d\n",
1154                                 __func__, output->state, line->id);
1155                 return;
1156         }
1157
1158         spin_lock_irqsave(&line->output_lock, flags);
1159
1160         active_index = output->active_buf;
1161
1162         ready_buf = output->buf[active_index];
1163         if (!ready_buf) {
1164                 st_err_ratelimited(ST_VIN,
1165                                         "Missing ready buf %d %d!\n",
1166                                         active_index, output->state);
1167                 active_index = !active_index;
1168                 ready_buf = output->buf[active_index];
1169                 if (!ready_buf) {
1170                         st_err_ratelimited(ST_VIN,
1171                                         "Missing ready buf 2 %d %d!\n",
1172                                         active_index, output->state);
1173                         goto out_unlock;
1174                 }
1175         }
1176
1177         /* Get next buffer */
1178         output->buf[active_index] = vin_buf_get_pending(output);
1179         if (!output->buf[active_index]) {
1180                 /* No next buffer - set same address */
1181                 new_addr = ready_buf->addr;
1182                 vin_buf_update_on_last(line);
1183         } else {
1184                 new_addr = output->buf[active_index]->addr;
1185                 vin_buf_update_on_next(line);
1186         }
1187
1188         if (output->state == VIN_OUTPUT_STOPPING)
1189                 output->last_buffer = ready_buf;
1190         else {
1191                 switch (stf_vin_map_isp_line(line->id)) {
1192                 case STF_ISP_LINE_SRC:
1193                         vin_dev->hw_ops->vin_isp_set_yuv_addr(vin_dev,
1194                                 new_addr[0], new_addr[1]);
1195                         break;
1196                 case STF_ISP_LINE_SRC_SS0:
1197                         vin_dev->hw_ops->vin_isp_set_ss0_addr(vin_dev,
1198                                 new_addr[0], new_addr[1]);
1199                         break;
1200                 case STF_ISP_LINE_SRC_SS1:
1201                         vin_dev->hw_ops->vin_isp_set_ss1_addr(vin_dev,
1202                                 new_addr[0], new_addr[1]);
1203                         break;
1204                 case STF_ISP_LINE_SRC_ITIW:
1205                         vin_dev->hw_ops->vin_isp_set_itiw_addr(vin_dev,
1206                                 new_addr[0], new_addr[1]);
1207                         break;
1208                 case STF_ISP_LINE_SRC_ITIR:
1209                         vin_dev->hw_ops->vin_isp_set_itir_addr(vin_dev,
1210                                 new_addr[0], new_addr[1]);
1211                         break;
1212                 case STF_ISP_LINE_SRC_RAW:
1213                         vin_dev->hw_ops->vin_isp_set_raw_addr(vin_dev, new_addr[0]);
1214                         break;
1215                 case STF_ISP_LINE_SRC_SCD_Y:
1216                         scd_type = vin_dev->hw_ops->vin_isp_get_scd_type(vin_dev);
1217                         ready_buf->vb.flags &= ~(V4L2_BUF_FLAG_PFRAME | V4L2_BUF_FLAG_BFRAME);
1218                         if (scd_type == AWB_TYPE)
1219                                 ready_buf->vb.flags |= V4L2_BUF_FLAG_PFRAME;
1220                         else
1221                                 ready_buf->vb.flags |= V4L2_BUF_FLAG_BFRAME;
1222                         if (!output->frame_skip) {
1223                                 output->frame_skip = ISP_AWB_OECF_SKIP_FRAME;
1224                                 scd_type = scd_type == AWB_TYPE ? OECF_TYPE : AWB_TYPE;
1225                         } else {
1226                                 output->frame_skip--;
1227                                 scd_type = scd_type == AWB_TYPE ? AWB_TYPE : OECF_TYPE;
1228                         }
1229                         vin_dev->hw_ops->vin_isp_set_scd_addr(vin_dev,
1230                                 new_addr[0], new_addr[1], scd_type);
1231                         break;
1232                 default:
1233                         if (line->id == VIN_LINE_WR) {
1234 #ifdef VIN_TWO_BUFFER
1235                                 if (active_index)
1236                                         vin_dev->hw_ops->vin_wr_set_pong_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 #else
1242                                 vin_dev->hw_ops->vin_wr_set_ping_addr(vin_dev,
1243                                                         new_addr[0]);
1244                                 vin_dev->hw_ops->vin_wr_set_pong_addr(vin_dev,
1245                                                         new_addr[0]);
1246 #endif
1247                         }
1248                         break;
1249                 }
1250
1251                 vin_buf_add_ready(output, ready_buf);
1252         }
1253
1254         spin_unlock_irqrestore(&line->output_lock, flags);
1255         return;
1256
1257 out_unlock:
1258         spin_unlock_irqrestore(&line->output_lock, flags);
1259 }
1260
1261 static int vin_queue_buffer(struct stfcamss_video *vid,
1262                                 struct stfcamss_buffer *buf)
1263 {
1264         struct vin_line *line = container_of(vid, struct vin_line, video_out);
1265         struct vin_output *output;
1266         unsigned long flags;
1267
1268
1269         output = &line->output;
1270
1271         spin_lock_irqsave(&line->output_lock, flags);
1272
1273         vin_buf_update_on_new(line, output, buf);
1274
1275         spin_unlock_irqrestore(&line->output_lock, flags);
1276
1277         return 0;
1278 }
1279
1280 static int vin_flush_buffers(struct stfcamss_video *vid,
1281                                 enum vb2_buffer_state state)
1282 {
1283         struct vin_line *line = container_of(vid, struct vin_line, video_out);
1284         struct vin_output *output = &line->output;
1285         unsigned long flags;
1286
1287         spin_lock_irqsave(&line->output_lock, flags);
1288
1289         vin_buf_flush(output, state);
1290         if (output->buf[0])
1291                 vb2_buffer_done(&output->buf[0]->vb.vb2_buf, state);
1292
1293         if (output->buf[1])
1294                 vb2_buffer_done(&output->buf[1]->vb.vb2_buf, state);
1295
1296         if (output->last_buffer) {
1297                 vb2_buffer_done(&output->last_buffer->vb.vb2_buf, state);
1298                 output->last_buffer = NULL;
1299         }
1300         output->buf[0] = output->buf[1] = NULL;
1301
1302         spin_unlock_irqrestore(&line->output_lock, flags);
1303         return 0;
1304 }
1305
1306 static int vin_link_setup(struct media_entity *entity,
1307                         const struct media_pad *local,
1308                         const struct media_pad *remote, u32 flags)
1309 {
1310         if (flags & MEDIA_LNK_FL_ENABLED)
1311                 if (media_entity_remote_pad(local))
1312                         return -EBUSY;
1313         return 0;
1314 }
1315
1316 static const struct v4l2_subdev_core_ops vin_core_ops = {
1317         .s_power = vin_set_power,
1318 };
1319
1320 static const struct v4l2_subdev_video_ops vin_video_ops = {
1321         .s_stream = vin_set_stream,
1322 };
1323
1324 static const struct v4l2_subdev_pad_ops vin_pad_ops = {
1325         .enum_mbus_code   = vin_enum_mbus_code,
1326         .enum_frame_size  = vin_enum_frame_size,
1327         .get_fmt          = vin_get_format,
1328         .set_fmt          = vin_set_format,
1329 };
1330
1331 static const struct v4l2_subdev_ops vin_v4l2_ops = {
1332         .core = &vin_core_ops,
1333         .video = &vin_video_ops,
1334         .pad = &vin_pad_ops,
1335 };
1336
1337 static const struct v4l2_subdev_internal_ops vin_v4l2_internal_ops = {
1338         .open = vin_init_formats,
1339 };
1340
1341 static const struct stfcamss_video_ops stfcamss_vin_video_ops = {
1342         .queue_buffer = vin_queue_buffer,
1343         .flush_buffers = vin_flush_buffers,
1344 };
1345
1346 static const struct media_entity_operations vin_media_ops = {
1347         .link_setup = vin_link_setup,
1348         .link_validate = v4l2_subdev_link_validate,
1349 };
1350
1351 int stf_vin_register(struct stf_vin2_dev *vin_dev, struct v4l2_device *v4l2_dev)
1352 {
1353         struct v4l2_subdev *sd;
1354         struct stfcamss_video *video_out;
1355         struct media_pad *pads;
1356         int ret;
1357         int i;
1358
1359         for (i = 0; i < STF_ISP_LINE_MAX + 1; i++) {
1360                 char name[32];
1361                 char *sub_name = get_line_subdevname(i);
1362                 int is_mp;
1363
1364                 is_mp = (stf_vin_map_isp_line(i) == STF_ISP_LINE_SRC) ? true : false;
1365                 is_mp = false;
1366                 sd = &vin_dev->line[i].subdev;
1367                 pads = vin_dev->line[i].pads;
1368                 video_out = &vin_dev->line[i].video_out;
1369                 video_out->id = i;
1370
1371                 v4l2_subdev_init(sd, &vin_v4l2_ops);
1372                 sd->internal_ops = &vin_v4l2_internal_ops;
1373                 sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
1374                 snprintf(sd->name, ARRAY_SIZE(sd->name), "%s%d_%s",
1375                         STF_VIN_NAME, 0, sub_name);
1376                 v4l2_set_subdevdata(sd, &vin_dev->line[i]);
1377
1378                 ret = vin_init_formats(sd, NULL);
1379                 if (ret < 0) {
1380                         st_err(ST_VIN, "Failed to init format: %d\n", ret);
1381                         goto err_init;
1382                 }
1383
1384                 pads[STF_VIN_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
1385                 pads[STF_VIN_PAD_SRC].flags = MEDIA_PAD_FL_SOURCE;
1386
1387                 sd->entity.function =
1388                         MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER;
1389                 sd->entity.ops = &vin_media_ops;
1390                 ret = media_entity_pads_init(&sd->entity,
1391                                 STF_VIN_PADS_NUM, pads);
1392                 if (ret < 0) {
1393                         st_err(ST_VIN, "Failed to init media entity: %d\n", ret);
1394                         goto err_init;
1395                 }
1396
1397                 ret = v4l2_device_register_subdev(v4l2_dev, sd);
1398                 if (ret < 0) {
1399                         st_err(ST_VIN, "Failed to register subdev: %d\n", ret);
1400                         goto err_reg_subdev;
1401                 }
1402
1403                 video_out->ops = &stfcamss_vin_video_ops;
1404                 video_out->bpl_alignment = 16 * 8;
1405
1406                 snprintf(name, ARRAY_SIZE(name), "%s_%s%d",
1407                         sd->name, "video", i);
1408                 ret = stf_video_register(video_out, v4l2_dev, name, is_mp);
1409                 if (ret < 0) {
1410                         st_err(ST_VIN, "Failed to register video node: %d\n",
1411                                         ret);
1412                         goto err_vid_reg;
1413                 }
1414
1415                 ret = media_create_pad_link(
1416                         &sd->entity, STF_VIN_PAD_SRC,
1417                         &video_out->vdev.entity, 0,
1418                         MEDIA_LNK_FL_IMMUTABLE | MEDIA_LNK_FL_ENABLED);
1419                 if (ret < 0) {
1420                         st_err(ST_VIN, "Failed to link %s->%s entities: %d\n",
1421                                 sd->entity.name, video_out->vdev.entity.name,
1422                                 ret);
1423                         goto err_create_link;
1424                 }
1425         }
1426
1427         return 0;
1428
1429 err_create_link:
1430         stf_video_unregister(video_out);
1431 err_vid_reg:
1432         v4l2_device_unregister_subdev(sd);
1433 err_reg_subdev:
1434         media_entity_cleanup(&sd->entity);
1435 err_init:
1436         for (i--; i >= 0; i--) {
1437                 sd = &vin_dev->line[i].subdev;
1438                 video_out = &vin_dev->line[i].video_out;
1439
1440                 stf_video_unregister(video_out);
1441                 v4l2_device_unregister_subdev(sd);
1442                 media_entity_cleanup(&sd->entity);
1443         }
1444         return ret;
1445 }
1446
1447 int stf_vin_unregister(struct stf_vin2_dev *vin_dev)
1448 {
1449         struct v4l2_subdev *sd;
1450         struct stfcamss_video *video_out;
1451         int i;
1452
1453         mutex_destroy(&vin_dev->power_lock);
1454         for (i = 0; i < STF_DUMMY_MODULE_NUMS; i++)
1455                 mutex_destroy(&vin_dev->dummy_buffer[i].stream_lock);
1456
1457         for (i = 0; i < STF_ISP_LINE_MAX + 1; i++) {
1458                 sd = &vin_dev->line[i].subdev;
1459                 video_out = &vin_dev->line[i].video_out;
1460
1461                 stf_video_unregister(video_out);
1462                 v4l2_device_unregister_subdev(sd);
1463                 media_entity_cleanup(&sd->entity);
1464                 mutex_destroy(&vin_dev->line[i].stream_lock);
1465                 mutex_destroy(&vin_dev->line[i].power_lock);
1466         }
1467         return 0;
1468 }