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

   1 // SPDX-License-Identifier: GPL-2.0
   2 /*
   3  * Copyright (C) 2021 StarFive Technology Co., Ltd.
   4  */
   5 #include "stfcamss.h"
   6 #include <media/v4l2-async.h>
   7 #include <media/v4l2-ctrls.h>
   8 #include <media/v4l2-device.h>
   9 #include <media/v4l2-event.h>
  10 #include <media/v4l2-fwnode.h>
  11 #include <media/v4l2-subdev.h>
  12 #include <linux/firmware.h>
  13 #include "stf_isp_ioctl.h"
  14 #include "stf_dmabuf.h"
  15
  16 static int user_config_isp;
  17 static int isp_set_selection(struct v4l2_subdev *sd,
  18                              struct v4l2_subdev_state *state,
  19                              struct v4l2_subdev_selection *sel);
  20
  21 static struct v4l2_rect *
  22 __isp_get_compose(struct stf_isp_dev *isp_dev,
  23                   struct v4l2_subdev_state *state,
  24                   enum v4l2_subdev_format_whence which);
  25
  26 static struct v4l2_rect *
  27 __isp_get_crop(struct stf_isp_dev *isp_dev,
  28                 struct v4l2_subdev_state *state,
  29                 enum v4l2_subdev_format_whence which);
  30
  31 static struct v4l2_rect *
  32 __isp_get_scale(struct stf_isp_dev *isp_dev,
  33                 struct v4l2_subdev_state *state,
  34                 struct v4l2_subdev_selection *sel);
  35
  36 static struct v4l2_rect *
  37 __isp_get_itiws(struct stf_isp_dev *isp_dev,
  38                 struct v4l2_subdev_state *state,
  39                 enum v4l2_subdev_format_whence which);
  40
  41 // sink format and raw format must one by one
  42 static const struct isp_format isp_formats_st7110_sink[] = {
  43         { MEDIA_BUS_FMT_SRGGB10_1X10, 10},
  44         { MEDIA_BUS_FMT_SGRBG10_1X10, 10},
  45         { MEDIA_BUS_FMT_SGBRG10_1X10, 10},
  46         { MEDIA_BUS_FMT_SBGGR10_1X10, 10},
  47 };
  48
  49 static const struct isp_format isp_formats_st7110_raw[] = {
  50         { MEDIA_BUS_FMT_SRGGB12_1X12, 12},
  51         { MEDIA_BUS_FMT_SGRBG12_1X12, 12},
  52         { MEDIA_BUS_FMT_SGBRG12_1X12, 12},
  53         { MEDIA_BUS_FMT_SBGGR12_1X12, 12},
  54 };
  55
  56 static const struct isp_format isp_formats_st7110_compat_10bit_raw[] = {
  57         { MEDIA_BUS_FMT_SRGGB10_1X10, 10},
  58         { MEDIA_BUS_FMT_SGRBG10_1X10, 10},
  59         { MEDIA_BUS_FMT_SGBRG10_1X10, 10},
  60         { MEDIA_BUS_FMT_SBGGR10_1X10, 10},
  61 };
  62
  63 static const struct isp_format isp_formats_st7110_compat_8bit_raw[] = {
  64         { MEDIA_BUS_FMT_SRGGB8_1X8, 8},
  65         { MEDIA_BUS_FMT_SGRBG8_1X8, 8},
  66         { MEDIA_BUS_FMT_SGBRG8_1X8, 8},
  67         { MEDIA_BUS_FMT_SBGGR8_1X8, 8},
  68 };
  69
  70 static const struct isp_format isp_formats_st7110_uo[] = {
  71         { MEDIA_BUS_FMT_Y12_1X12, 8},
  72 };
  73
  74 static const struct isp_format isp_formats_st7110_iti[] = {
  75         { MEDIA_BUS_FMT_SRGGB10_1X10, 10},
  76         { MEDIA_BUS_FMT_SGRBG10_1X10, 10},
  77         { MEDIA_BUS_FMT_SGBRG10_1X10, 10},
  78         { MEDIA_BUS_FMT_SBGGR10_1X10, 10},
  79         { MEDIA_BUS_FMT_SRGGB12_1X12, 12},
  80         { MEDIA_BUS_FMT_SGRBG12_1X12, 12},
  81         { MEDIA_BUS_FMT_SGBRG12_1X12, 12},
  82         { MEDIA_BUS_FMT_SBGGR12_1X12, 12},
  83         { MEDIA_BUS_FMT_Y12_1X12, 8},
  84         { MEDIA_BUS_FMT_YUV8_1X24, 8},
  85 };
  86
  87 #define SINK_FORMATS_INDEX    0
  88 #define UO_FORMATS_INDEX      1
  89 #define ITI_FORMATS_INDEX     2
  90 #define RAW_FORMATS_INDEX     3
  91
  92 static const struct isp_format_table isp_formats_st7110[] = {
  93         { isp_formats_st7110_sink, ARRAY_SIZE(isp_formats_st7110_sink) }, // 0
  94         { isp_formats_st7110_uo, ARRAY_SIZE(isp_formats_st7110_uo) },     // 1
  95         { isp_formats_st7110_iti, ARRAY_SIZE(isp_formats_st7110_iti) },   // 2
  96         { isp_formats_st7110_raw, ARRAY_SIZE(isp_formats_st7110_raw) },   // 3
  97 };
  98
  99 int stf_isp_subdev_init(struct stfcamss *stfcamss)
 100 {
 101         struct stf_isp_dev *isp_dev = stfcamss->isp_dev;
 102
 103         isp_dev->sdev_type = ISP_DEV_TYPE;
 104         isp_dev->hw_ops = &isp_ops;
 105         isp_dev->stfcamss = stfcamss;
 106         isp_dev->formats = isp_formats_st7110;
 107         isp_dev->nformats = ARRAY_SIZE(isp_formats_st7110);
 108         mutex_init(&isp_dev->stream_lock);
 109         mutex_init(&isp_dev->power_lock);
 110         mutex_init(&isp_dev->setfile_lock);
 111         atomic_set(&isp_dev->shadow_count, 0);
 112         return 0;
 113 }
 114
 115 /*
 116  * ISP Controls.
 117  */
 118
 119 static inline struct v4l2_subdev *ctrl_to_sd(struct v4l2_ctrl *ctrl)
 120 {
 121         return &container_of(ctrl->handler, struct stf_isp_dev,
 122                              ctrls.handler)->subdev;
 123 }
 124
 125 static u64 isp_calc_pixel_rate(struct stf_isp_dev *isp_dev)
 126 {
 127         u64 rate = 0;
 128
 129         return rate;
 130 }
 131
 132 static int isp_set_ctrl_hue(struct stf_isp_dev *isp_dev, int value)
 133 {
 134         int ret = 0;
 135
 136         return ret;
 137 }
 138
 139 static int isp_set_ctrl_contrast(struct stf_isp_dev *isp_dev, int value)
 140 {
 141         int ret = 0;
 142
 143         return ret;
 144 }
 145
 146 static int isp_set_ctrl_saturation(struct stf_isp_dev *isp_dev, int value)
 147 {
 148         int ret = 0;
 149
 150         return ret;
 151 }
 152
 153 static int isp_set_ctrl_white_balance(struct stf_isp_dev *isp_dev, int awb)
 154 {
 155         struct isp_ctrls *ctrls = &isp_dev->ctrls;
 156         int ret = 0;
 157
 158         if (!awb && (ctrls->red_balance->is_new
 159                         || ctrls->blue_balance->is_new)) {
 160                 u16 red = (u16)ctrls->red_balance->val;
 161                 u16 blue = (u16)ctrls->blue_balance->val;
 162
 163                 st_debug(ST_ISP, "red = 0x%x, blue = 0x%x\n", red, blue);
 164                 //isp_dev->hw_ops->isp_set_awb_r_gain(isp_dev, red);
 165                 //if (ret)
 166                 //      return ret;
 167                 //isp_dev->hw_ops->isp_set_awb_b_gain(isp_dev, blue);
 168         }
 169
 170         return ret;
 171 }
 172
 173 static int isp_set_ctrl_exposure(struct stf_isp_dev *isp_dev,
 174                                     enum v4l2_exposure_auto_type auto_exposure)
 175 {
 176         int ret = 0;
 177
 178         return ret;
 179 }
 180
 181 static int isp_set_ctrl_gain(struct stf_isp_dev *isp_dev, bool auto_gain)
 182 {
 183         int ret = 0;
 184
 185         return ret;
 186 }
 187
 188 static const char * const test_pattern_menu[] = {
 189         "Disabled",
 190         "Color bars",
 191         "Color bars w/ rolling bar",
 192         "Color squares",
 193         "Color squares w/ rolling bar",
 194 };
 195
 196 #define ISP_TEST_ENABLE                 BIT(7)
 197 #define ISP_TEST_ROLLING                BIT(6)  /* rolling horizontal bar */
 198 #define ISP_TEST_TRANSPARENT            BIT(5)
 199 #define ISP_TEST_SQUARE_BW              BIT(4)  /* black & white squares */
 200 #define ISP_TEST_BAR_STANDARD           (0 << 2)
 201 #define ISP_TEST_BAR_VERT_CHANGE_1      (1 << 2)
 202 #define ISP_TEST_BAR_HOR_CHANGE         (2 << 2)
 203 #define ISP_TEST_BAR_VERT_CHANGE_2      (3 << 2)
 204 #define ISP_TEST_BAR                    (0 << 0)
 205 #define ISP_TEST_RANDOM                 (1 << 0)
 206 #define ISP_TEST_SQUARE                 (2 << 0)
 207 #define ISP_TEST_BLACK                  (3 << 0)
 208
 209 static const u8 test_pattern_val[] = {
 210         0,
 211         ISP_TEST_ENABLE | ISP_TEST_BAR_VERT_CHANGE_1 |
 212                 ISP_TEST_BAR,
 213         ISP_TEST_ENABLE | ISP_TEST_ROLLING |
 214                 ISP_TEST_BAR_VERT_CHANGE_1 | ISP_TEST_BAR,
 215         ISP_TEST_ENABLE | ISP_TEST_SQUARE,
 216         ISP_TEST_ENABLE | ISP_TEST_ROLLING | ISP_TEST_SQUARE,
 217 };
 218
 219 static int isp_set_ctrl_test_pattern(struct stf_isp_dev *isp_dev, int value)
 220 {
 221         int ret = 0;
 222
 223         // return isp_write_reg(isp_dev, ISP_REG_PRE_ISP_TEST_SET1,
 224         //                      test_pattern_val[value]);
 225         return ret;
 226 }
 227
 228 static int isp_set_ctrl_light_freq(struct stf_isp_dev *isp_dev, int value)
 229 {
 230         int ret = 0;
 231
 232         return ret;
 233 }
 234
 235 static int isp_set_ctrl_hflip(struct stf_isp_dev *isp_dev, int value)
 236 {
 237         int ret = 0;
 238
 239         return ret;
 240 }
 241
 242 static int isp_set_ctrl_vflip(struct stf_isp_dev *isp_dev, int value)
 243 {
 244         int ret = 0;
 245
 246         return ret;
 247 }
 248
 249 static int isp_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
 250 {
 251         switch (ctrl->id) {
 252         case V4L2_CID_AUTOGAIN:
 253                 break;
 254         case V4L2_CID_EXPOSURE_AUTO:
 255                 break;
 256         }
 257
 258         return 0;
 259 }
 260
 261 static int isp_s_ctrl(struct v4l2_ctrl *ctrl)
 262 {
 263         struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
 264         struct stf_isp_dev *isp_dev = v4l2_get_subdevdata(sd);
 265         int ret;
 266
 267         /*
 268          * If the device is not powered up by the host driver do
 269          * not apply any controls to H/W at this time. Instead
 270          * the controls will be restored right after power-up.
 271          */
 272         mutex_lock(&isp_dev->power_lock);
 273         if (isp_dev->power_count == 0) {
 274                 mutex_unlock(&isp_dev->power_lock);
 275                 return 0;
 276         }
 277         mutex_unlock(&isp_dev->power_lock);
 278
 279         switch (ctrl->id) {
 280         case V4L2_CID_AUTOGAIN:
 281                 ret = isp_set_ctrl_gain(isp_dev, ctrl->val);
 282                 break;
 283         case V4L2_CID_EXPOSURE_AUTO:
 284                 ret = isp_set_ctrl_exposure(isp_dev, ctrl->val);
 285                 break;
 286         case V4L2_CID_AUTO_WHITE_BALANCE:
 287                 ret = isp_set_ctrl_white_balance(isp_dev, ctrl->val);
 288                 break;
 289         case V4L2_CID_HUE:
 290                 ret = isp_set_ctrl_hue(isp_dev, ctrl->val);
 291                 break;
 292         case V4L2_CID_CONTRAST:
 293                 ret = isp_set_ctrl_contrast(isp_dev, ctrl->val);
 294                 break;
 295         case V4L2_CID_SATURATION:
 296                 ret = isp_set_ctrl_saturation(isp_dev, ctrl->val);
 297                 break;
 298         case V4L2_CID_TEST_PATTERN:
 299                 ret = isp_set_ctrl_test_pattern(isp_dev, ctrl->val);
 300                 break;
 301         case V4L2_CID_POWER_LINE_FREQUENCY:
 302                 ret = isp_set_ctrl_light_freq(isp_dev, ctrl->val);
 303                 break;
 304         case V4L2_CID_HFLIP:
 305                 ret = isp_set_ctrl_hflip(isp_dev, ctrl->val);
 306                 break;
 307         case V4L2_CID_VFLIP:
 308                 ret = isp_set_ctrl_vflip(isp_dev, ctrl->val);
 309                 break;
 310         default:
 311                 ret = -EINVAL;
 312                 break;
 313         }
 314
 315         return ret;
 316 }
 317
 318 static const struct v4l2_ctrl_ops isp_ctrl_ops = {
 319         .g_volatile_ctrl = isp_g_volatile_ctrl,
 320         .s_ctrl = isp_s_ctrl,
 321 };
 322
 323 static int isp_init_controls(struct stf_isp_dev *isp_dev)
 324 {
 325         const struct v4l2_ctrl_ops *ops = &isp_ctrl_ops;
 326         struct isp_ctrls *ctrls = &isp_dev->ctrls;
 327         struct v4l2_ctrl_handler *hdl = &ctrls->handler;
 328         int ret;
 329
 330         v4l2_ctrl_handler_init(hdl, 32);
 331
 332         /* Clock related controls */
 333         ctrls->pixel_rate = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_PIXEL_RATE,
 334                                               0, INT_MAX, 1,
 335                                               isp_calc_pixel_rate(isp_dev));
 336
 337         /* Auto/manual white balance */
 338         ctrls->auto_wb = v4l2_ctrl_new_std(hdl, ops,
 339                                            V4L2_CID_AUTO_WHITE_BALANCE,
 340                                            0, 1, 1, 1);
 341         ctrls->blue_balance = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_BLUE_BALANCE,
 342                                                 0, 4095, 1, 0);
 343         ctrls->red_balance = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_RED_BALANCE,
 344                                                0, 4095, 1, 0);
 345         /* Auto/manual exposure */
 346         ctrls->auto_exp = v4l2_ctrl_new_std_menu(hdl, ops,
 347                                                  V4L2_CID_EXPOSURE_AUTO,
 348                                                  V4L2_EXPOSURE_MANUAL, 0,
 349                                                  V4L2_EXPOSURE_AUTO);
 350         ctrls->exposure = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_EXPOSURE,
 351                                             0, 65535, 1, 0);
 352         /* Auto/manual gain */
 353         ctrls->auto_gain = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_AUTOGAIN,
 354                                              0, 1, 1, 1);
 355         ctrls->gain = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_GAIN,
 356                                         0, 1023, 1, 0);
 357
 358         ctrls->saturation = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_SATURATION,
 359                                               0, 255, 1, 64);
 360         ctrls->hue = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HUE,
 361                                        0, 359, 1, 0);
 362         ctrls->contrast = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_CONTRAST,
 363                                             0, 255, 1, 0);
 364         ctrls->test_pattern =
 365                 v4l2_ctrl_new_std_menu_items(hdl, ops, V4L2_CID_TEST_PATTERN,
 366                                              ARRAY_SIZE(test_pattern_menu) - 1,
 367                                              0, 0, test_pattern_menu);
 368         ctrls->hflip = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HFLIP,
 369                                          0, 1, 1, 0);
 370         ctrls->vflip = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_VFLIP,
 371                                          0, 1, 1, 0);
 372
 373         ctrls->light_freq =
 374                 v4l2_ctrl_new_std_menu(hdl, ops,
 375                                        V4L2_CID_POWER_LINE_FREQUENCY,
 376                                        V4L2_CID_POWER_LINE_FREQUENCY_AUTO, 0,
 377                                        V4L2_CID_POWER_LINE_FREQUENCY_50HZ);
 378
 379         if (hdl->error) {
 380                 ret = hdl->error;
 381                 goto free_ctrls;
 382         }
 383
 384         ctrls->pixel_rate->flags |= V4L2_CTRL_FLAG_READ_ONLY;
 385         ctrls->gain->flags |= V4L2_CTRL_FLAG_VOLATILE;
 386         ctrls->exposure->flags |= V4L2_CTRL_FLAG_VOLATILE;
 387
 388         v4l2_ctrl_auto_cluster(3, &ctrls->auto_wb, 0, false);
 389         v4l2_ctrl_auto_cluster(2, &ctrls->auto_gain, 0, true);
 390         v4l2_ctrl_auto_cluster(2, &ctrls->auto_exp, 1, true);
 391
 392         isp_dev->subdev.ctrl_handler = hdl;
 393         return 0;
 394
 395 free_ctrls:
 396         v4l2_ctrl_handler_free(hdl);
 397         return ret;
 398 }
 399
 400 static int isp_set_power(struct v4l2_subdev *sd, int on)
 401 {
 402         struct stf_isp_dev *isp_dev = v4l2_get_subdevdata(sd);
 403         struct stf_vin2_dev *vin_dev = isp_dev->stfcamss->vin_dev;
 404
 405         st_debug(ST_ISP, "%s, %d\n", __func__, __LINE__);
 406         mutex_lock(&isp_dev->power_lock);
 407         if (on) {
 408                 if (isp_dev->power_count == 0) {
 409                         /* Needs to enable vin clock before access ISP. */
 410                         vin_dev->hw_ops->vin_top_clk_init(vin_dev);
 411                         vin_dev->hw_ops->vin_clk_enable(vin_dev);
 412                         isp_dev->hw_ops->isp_clk_enable(isp_dev);
 413                         if (!user_config_isp)
 414                                 isp_dev->hw_ops->isp_config_set(isp_dev);
 415                 }
 416                 isp_dev->power_count++;
 417         } else {
 418                 if (isp_dev->power_count == 0)
 419                         goto exit;
 420                 if (isp_dev->power_count == 1)
 421                         isp_dev->hw_ops->isp_clk_disable(isp_dev);
 422                 isp_dev->power_count--;
 423         }
 424 exit:
 425         mutex_unlock(&isp_dev->power_lock);
 426
 427         return 0;
 428 }
 429
 430 static struct v4l2_mbus_framefmt *
 431 __isp_get_format(struct stf_isp_dev *isp_dev,
 432                 struct v4l2_subdev_state *state,
 433                 unsigned int pad,
 434                 enum v4l2_subdev_format_whence which)
 435 {
 436
 437         if (which == V4L2_SUBDEV_FORMAT_TRY)
 438                 return v4l2_subdev_get_try_format(&isp_dev->subdev, state, pad);
 439
 440         return &isp_dev->fmt[pad];
 441 }
 442
 443 static int isp_get_interface_type(struct media_entity *entity)
 444 {
 445         struct v4l2_subdev *subdev;
 446         struct media_pad *pad = &entity->pads[0];
 447
 448         if (!(pad->flags & MEDIA_PAD_FL_SINK))
 449                 return -EINVAL;
 450
 451         pad = media_entity_remote_pad(pad);
 452         if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
 453                 return -EINVAL;
 454
 455         subdev = media_entity_to_v4l2_subdev(pad->entity);
 456
 457         st_debug(ST_ISP, "interface subdev name %s\n", subdev->name);
 458         if (!strncmp(subdev->name, STF_CSI_NAME, strlen(STF_CSI_NAME)))
 459                 return CSI_SENSOR;
 460         if (!strncmp(subdev->name, STF_DVP_NAME, strlen(STF_DVP_NAME)))
 461                 return DVP_SENSOR;
 462         return -EINVAL;
 463 }
 464
 465 static int isp_set_stream(struct v4l2_subdev *sd, int enable)
 466 {
 467         struct stf_isp_dev *isp_dev = v4l2_get_subdevdata(sd);
 468         int ret = 0, interface_type;
 469         struct v4l2_mbus_framefmt *fmt;
 470         struct v4l2_event src_ch = { 0 };
 471
 472         fmt = __isp_get_format(isp_dev, NULL, STF_ISP_PAD_SINK, V4L2_SUBDEV_FORMAT_ACTIVE);
 473         mutex_lock(&isp_dev->stream_lock);
 474         if (enable) {
 475                 if (isp_dev->stream_count == 0) {
 476                         interface_type = isp_get_interface_type(&sd->entity);
 477                         if (interface_type < 0) {
 478                                 st_err(ST_ISP, "%s, pipeline not config\n", __func__);
 479                                 goto exit;
 480                         }
 481                         isp_dev->hw_ops->isp_set_format(isp_dev,
 482                                         isp_dev->rect, fmt->code, interface_type);
 483                         isp_dev->hw_ops->isp_reset(isp_dev);
 484                         isp_dev->hw_ops->isp_stream_set(isp_dev, enable);
 485                         user_config_isp = 0;
 486                 }
 487                 isp_dev->stream_count++;
 488         } else {
 489                 if (isp_dev->stream_count == 0)
 490                         goto exit;
 491                 if (isp_dev->stream_count == 1)
 492                         isp_dev->hw_ops->isp_stream_set(isp_dev, enable);
 493                 isp_dev->stream_count--;
 494         }
 495         src_ch.type = V4L2_EVENT_SOURCE_CHANGE,
 496         src_ch.u.src_change.changes = isp_dev->stream_count,
 497
 498         v4l2_subdev_notify_event(sd, &src_ch);
 499 exit:
 500         mutex_unlock(&isp_dev->stream_lock);
 501
 502         mutex_lock(&isp_dev->power_lock);
 503         /* restore controls */
 504         if (enable && isp_dev->power_count == 1) {
 505                 mutex_unlock(&isp_dev->power_lock);
 506                 ret = v4l2_ctrl_handler_setup(&isp_dev->ctrls.handler);
 507         } else
 508                 mutex_unlock(&isp_dev->power_lock);
 509
 510         return ret;
 511 }
 512
 513 /*Try to match sensor format with sink, and then get the index as default.*/
 514 static int isp_match_sensor_format_get_index(struct stf_isp_dev *isp_dev)
 515 {
 516         int ret, idx;
 517         struct media_entity *sensor;
 518         struct v4l2_subdev *subdev;
 519         struct v4l2_subdev_format fmt;
 520         const struct isp_format_table *formats;
 521
 522         if (!isp_dev)
 523                 return -EINVAL;
 524
 525         sensor = stfcamss_find_sensor(&isp_dev->subdev.entity);
 526         if (!sensor)
 527                 return -EINVAL;
 528
 529         subdev = media_entity_to_v4l2_subdev(sensor);
 530         st_debug(ST_ISP, "Found sensor = %s\n", sensor->name);
 531
 532         fmt.pad = 0;
 533         fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
 534         ret = v4l2_subdev_call(subdev, pad, get_fmt, NULL, &fmt);
 535         if (ret) {
 536                 st_warn(ST_ISP, "Sonser get format failed !!\n");
 537                 return -EINVAL;
 538         }
 539
 540         st_debug(ST_ISP, "Got sensor format 0x%x !!\n", fmt.format.code);
 541
 542         formats = &isp_dev->formats[SINK_FORMATS_INDEX];
 543         for (idx = 0; idx < formats->nfmts; idx++) {
 544                 if (formats->fmts[idx].code == fmt.format.code) {
 545                         st_info(ST_ISP,
 546                                 "Match sensor format to isp_formats_st7110_sink index %d !!\n",
 547                                 idx);
 548                         return idx;
 549                 }
 550         }
 551         return -ERANGE;
 552 }
 553
 554 static int isp_match_format_get_index(const struct isp_format_table *f_table,
 555                         __u32 mbus_code,
 556                         unsigned int pad)
 557 {
 558         int i;
 559
 560         for (i = 0; i < f_table->nfmts; i++) {
 561                 if (mbus_code == f_table->fmts[i].code) {
 562                         break;
 563                 } else {
 564                         if (pad == STF_ISP_PAD_SRC_RAW || pad == STF_ISP_PAD_SRC_SCD_Y) {
 565                                 if (mbus_code == (isp_formats_st7110_compat_10bit_raw[i].code ||
 566                                         isp_formats_st7110_compat_8bit_raw[i].code))
 567                                         break;
 568                         }
 569                 }
 570         }
 571
 572         return i;
 573 }
 574
 575 static void isp_try_format(struct stf_isp_dev *isp_dev,
 576                         struct v4l2_subdev_state *state,
 577                         unsigned int pad,
 578                         struct v4l2_mbus_framefmt *fmt,
 579                         enum v4l2_subdev_format_whence which)
 580 {
 581         const struct isp_format_table *formats;
 582         unsigned int i;
 583         u32 code = fmt->code;
 584         u32 bpp;
 585
 586         switch (pad) {
 587         case STF_ISP_PAD_SINK:
 588                 /* Set format on sink pad */
 589
 590                 formats = &isp_dev->formats[SINK_FORMATS_INDEX];
 591                 fmt->width = clamp_t(u32,
 592                                 fmt->width, STFCAMSS_FRAME_MIN_WIDTH,
 593                                 STFCAMSS_FRAME_MAX_WIDTH);
 594                 fmt->height = clamp_t(u32,
 595                                 fmt->height, STFCAMSS_FRAME_MIN_HEIGHT,
 596                                 STFCAMSS_FRAME_MAX_HEIGHT);
 597                 fmt->height &= ~0x1;
 598
 599                 fmt->field = V4L2_FIELD_NONE;
 600                 fmt->colorspace = V4L2_COLORSPACE_SRGB;
 601                 fmt->flags = 0;
 602
 603                 break;
 604
 605         case STF_ISP_PAD_SRC:
 606         case STF_ISP_PAD_SRC_SS0:
 607         case STF_ISP_PAD_SRC_SS1:
 608                 formats = &isp_dev->formats[UO_FORMATS_INDEX];
 609                 break;
 610
 611         case STF_ISP_PAD_SRC_ITIW:
 612         case STF_ISP_PAD_SRC_ITIR:
 613                 formats = &isp_dev->formats[ITI_FORMATS_INDEX];
 614                 break;
 615
 616         case STF_ISP_PAD_SRC_RAW:
 617         case STF_ISP_PAD_SRC_SCD_Y:
 618                 formats = &isp_dev->formats[RAW_FORMATS_INDEX];
 619                 break;
 620         }
 621
 622         i = isp_match_format_get_index(formats, fmt->code, pad);
 623         st_debug(ST_ISP, "%s pad=%d, code=%x isp_match_format_get_index = %d\n",
 624                                         __func__, pad, code, i);
 625
 626         if (i >= formats->nfmts &&
 627                 (pad == STF_ISP_PAD_SRC_RAW || pad == STF_ISP_PAD_SRC_SCD_Y)) {
 628                 int sensor_idx;
 629
 630                 sensor_idx = isp_match_sensor_format_get_index(isp_dev);
 631                 if (sensor_idx)
 632                         i = sensor_idx;
 633         }
 634
 635         if (pad != STF_ISP_PAD_SINK)
 636                 *fmt = *__isp_get_format(isp_dev, state, STF_ISP_PAD_SINK, which);
 637
 638         if (i >= formats->nfmts) {
 639                 fmt->code = formats->fmts[0].code;
 640                 bpp = formats->fmts[0].bpp;
 641                 st_info(ST_ISP, "Use default index 0 format = 0x%x\n", fmt->code);
 642         } else {
 643                 // sink format and raw format must one by one
 644                 if (pad == STF_ISP_PAD_SRC_RAW || pad == STF_ISP_PAD_SRC_SCD_Y) {
 645                         fmt->code = formats->fmts[i].code;
 646                         bpp = formats->fmts[i].bpp;
 647                         st_info(ST_ISP, "Use mapping format from sink index %d = 0x%x\n",
 648                                         i, fmt->code);
 649                 } else {
 650                         fmt->code = code;
 651                         bpp = formats->fmts[i].bpp;
 652                         st_info(ST_ISP, "Use input format = 0x%x\n", fmt->code);
 653                 }
 654         }
 655
 656         switch (pad) {
 657         case STF_ISP_PAD_SINK:
 658                 break;
 659         case STF_ISP_PAD_SRC:
 660                 isp_dev->rect[ISP_COMPOSE].bpp = bpp;
 661                 break;
 662         case STF_ISP_PAD_SRC_SS0:
 663                 isp_dev->rect[ISP_SCALE_SS0].bpp = bpp;
 664                 break;
 665         case STF_ISP_PAD_SRC_SS1:
 666                 isp_dev->rect[ISP_SCALE_SS1].bpp = bpp;
 667                 break;
 668         case STF_ISP_PAD_SRC_ITIW:
 669         case STF_ISP_PAD_SRC_ITIR:
 670                 isp_dev->rect[ISP_ITIWS].bpp = bpp;
 671                 break;
 672         case STF_ISP_PAD_SRC_RAW:
 673                 isp_dev->rect[ISP_CROP].bpp = bpp;
 674                 break;
 675         case STF_ISP_PAD_SRC_SCD_Y:
 676                 break;
 677         }
 678 }
 679
 680 static int isp_enum_mbus_code(struct v4l2_subdev *sd,
 681                         struct v4l2_subdev_state *state,
 682                         struct v4l2_subdev_mbus_code_enum *code)
 683 {
 684         struct stf_isp_dev *isp_dev = v4l2_get_subdevdata(sd);
 685         const struct isp_format_table *formats;
 686
 687         if (code->index >= isp_dev->nformats)
 688                 return -EINVAL;
 689         if (code->pad == STF_ISP_PAD_SINK) {
 690                 formats = &isp_dev->formats[SINK_FORMATS_INDEX];
 691                 code->code = formats->fmts[code->index].code;
 692         } else {
 693                 struct v4l2_mbus_framefmt *sink_fmt;
 694
 695                 sink_fmt = __isp_get_format(isp_dev, state, STF_ISP_PAD_SINK,
 696                                         code->which);
 697
 698                 code->code = sink_fmt->code;
 699                 if (!code->code)
 700                         return -EINVAL;
 701         }
 702         code->flags = 0;
 703
 704         return 0;
 705 }
 706
 707 static int isp_enum_frame_size(struct v4l2_subdev *sd,
 708                                 struct v4l2_subdev_state *state,
 709                                 struct v4l2_subdev_frame_size_enum *fse)
 710 {
 711         struct stf_isp_dev *isp_dev = v4l2_get_subdevdata(sd);
 712         struct v4l2_mbus_framefmt format;
 713
 714         if (fse->index != 0)
 715                 return -EINVAL;
 716
 717         format.code = fse->code;
 718         format.width = 1;
 719         format.height = 1;
 720         isp_try_format(isp_dev, state, fse->pad, &format, fse->which);
 721         fse->min_width = format.width;
 722         fse->min_height = format.height;
 723
 724         if (format.code != fse->code)
 725                 return -EINVAL;
 726
 727         format.code = fse->code;
 728         format.width = -1;
 729         format.height = -1;
 730         isp_try_format(isp_dev, state, fse->pad, &format, fse->which);
 731         fse->max_width = format.width;
 732         fse->max_height = format.height;
 733
 734         return 0;
 735 }
 736
 737 static int isp_get_format(struct v4l2_subdev *sd,
 738                         struct v4l2_subdev_state *state,
 739                         struct v4l2_subdev_format *fmt)
 740 {
 741         struct stf_isp_dev *isp_dev = v4l2_get_subdevdata(sd);
 742         struct v4l2_mbus_framefmt *format;
 743
 744         format = __isp_get_format(isp_dev, state, fmt->pad, fmt->which);
 745         if (format == NULL)
 746                 return -EINVAL;
 747
 748         fmt->format = *format;
 749
 750         return 0;
 751 }
 752
 753 static int isp_set_format(struct v4l2_subdev *sd,
 754                         struct v4l2_subdev_state *state,
 755                         struct v4l2_subdev_format *fmt)
 756 {
 757         struct stf_isp_dev *isp_dev = v4l2_get_subdevdata(sd);
 758         struct v4l2_mbus_framefmt *format;
 759         struct v4l2_subdev_selection sel = { 0 };
 760         struct v4l2_rect *rect = NULL;
 761         int ret;
 762
 763         st_debug(ST_ISP, "%s pad=%d, code=%x, which=%d\n",
 764                         __func__, fmt->reserved[0], fmt->format.code, fmt->which);
 765         format = __isp_get_format(isp_dev, state, fmt->pad, fmt->which);
 766         if (format == NULL)
 767                 return -EINVAL;
 768
 769         mutex_lock(&isp_dev->stream_lock);
 770         if (isp_dev->stream_count) {
 771                 fmt->format = *format;
 772                 if (fmt->reserved[0] != 0) {
 773                         sel.which = fmt->which;
 774                         sel.pad = fmt->reserved[0];
 775
 776                         switch (fmt->reserved[0]) {
 777                         case STF_ISP_PAD_SRC:
 778                                 rect = __isp_get_compose(isp_dev, state, fmt->which);
 779                                 break;
 780                         case STF_ISP_PAD_SRC_SS0:
 781                         case STF_ISP_PAD_SRC_SS1:
 782                                 rect = __isp_get_scale(isp_dev, state, &sel);
 783                                 break;
 784                         case STF_ISP_PAD_SRC_ITIW:
 785                         case STF_ISP_PAD_SRC_ITIR:
 786                                 rect = __isp_get_itiws(isp_dev, state, fmt->which);
 787                                 break;
 788                         case STF_ISP_PAD_SRC_RAW:
 789                         case STF_ISP_PAD_SRC_SCD_Y:
 790                                 rect = __isp_get_crop(isp_dev, state, fmt->which);
 791                                 break;
 792                         default:
 793                                 break;
 794                         }
 795                         if (rect != NULL) {
 796                                 fmt->format.width = rect->width;
 797                                 fmt->format.height = rect->height;
 798                         }
 799                 }
 800                 mutex_unlock(&isp_dev->stream_lock);
 801                 goto out;
 802         } else {
 803                 isp_try_format(isp_dev, state, fmt->pad, &fmt->format, fmt->which);
 804                 *format = fmt->format;
 805         }
 806         mutex_unlock(&isp_dev->stream_lock);
 807
 808         /* Propagate the format from sink to source */
 809         if (fmt->pad == STF_ISP_PAD_SINK) {
 810                 /* Reset sink pad compose selection */
 811                 sel.which = fmt->which;
 812                 sel.pad = STF_ISP_PAD_SINK;
 813                 sel.target = V4L2_SEL_TGT_CROP;
 814                 sel.r.width = fmt->format.width;
 815                 sel.r.height = fmt->format.height;
 816                 ret = isp_set_selection(sd, state, &sel);
 817                 if (ret < 0)
 818                         return ret;
 819         }
 820
 821 out:
 822         return 0;
 823 }
 824
 825 static struct v4l2_rect *
 826 __isp_get_compose(struct stf_isp_dev *isp_dev,
 827                   struct v4l2_subdev_state *state,
 828                   enum v4l2_subdev_format_whence which)
 829 {
 830         if (which == V4L2_SUBDEV_FORMAT_TRY)
 831                 return v4l2_subdev_get_try_compose(&isp_dev->subdev, state,
 832                                                    STF_ISP_PAD_SINK);
 833
 834
 835         return &isp_dev->rect[ISP_COMPOSE].rect;
 836 }
 837
 838 static struct v4l2_rect *
 839 __isp_get_crop(struct stf_isp_dev *isp_dev,
 840                 struct v4l2_subdev_state *state,
 841                 enum v4l2_subdev_format_whence which)
 842 {
 843         if (which == V4L2_SUBDEV_FORMAT_TRY)
 844                 return v4l2_subdev_get_try_crop(&isp_dev->subdev, state,
 845                                                 STF_ISP_PAD_SINK);
 846
 847         return &isp_dev->rect[ISP_CROP].rect;
 848 }
 849
 850 static struct v4l2_rect *
 851 __isp_get_scale(struct stf_isp_dev *isp_dev,
 852                 struct v4l2_subdev_state *state,
 853                 struct v4l2_subdev_selection *sel)
 854 {
 855         int pad;
 856
 857         if (sel->which == V4L2_SUBDEV_FORMAT_TRY)
 858                 return v4l2_subdev_get_try_compose(&isp_dev->subdev, state,
 859                                                 STF_ISP_PAD_SINK);
 860         if (sel->pad != STF_ISP_PAD_SRC_SS0 && sel->pad != STF_ISP_PAD_SRC_SS1)
 861                 return NULL;
 862
 863         pad = sel->pad == STF_ISP_PAD_SRC_SS0 ? ISP_SCALE_SS0 : ISP_SCALE_SS1;
 864         return &isp_dev->rect[pad].rect;
 865 }
 866
 867 static struct v4l2_rect *
 868 __isp_get_itiws(struct stf_isp_dev *isp_dev,
 869                 struct v4l2_subdev_state *state,
 870                 enum v4l2_subdev_format_whence which)
 871 {
 872         if (which == V4L2_SUBDEV_FORMAT_TRY)
 873                 return v4l2_subdev_get_try_crop(&isp_dev->subdev, state, STF_ISP_PAD_SINK);
 874
 875         return &isp_dev->rect[ISP_ITIWS].rect;
 876 }
 877
 878 static void isp_try_crop(struct stf_isp_dev *isp_dev,
 879                             struct v4l2_subdev_state *state,
 880                             struct v4l2_rect *rect,
 881                             enum v4l2_subdev_format_whence which)
 882 {
 883         struct v4l2_mbus_framefmt *fmt;
 884
 885         fmt = __isp_get_format(isp_dev, state, STF_ISP_PAD_SINK, which);
 886
 887         if (rect->width > fmt->width)
 888                 rect->width = fmt->width;
 889
 890         if (rect->width + rect->left > fmt->width)
 891                 rect->left = fmt->width - rect->width;
 892
 893         if (rect->height > fmt->height)
 894                 rect->height = fmt->height;
 895
 896         if (rect->height + rect->top > fmt->height)
 897                 rect->top = fmt->height - rect->height;
 898
 899         if (rect->width < STFCAMSS_FRAME_MIN_WIDTH) {
 900                 rect->left = 0;
 901                 rect->width = STFCAMSS_FRAME_MIN_WIDTH;
 902         }
 903
 904         if (rect->height < STFCAMSS_FRAME_MIN_HEIGHT) {
 905                 rect->top = 0;
 906                 rect->height = STFCAMSS_FRAME_MIN_HEIGHT;
 907         }
 908         rect->height &= ~0x1;
 909 }
 910
 911 static void isp_try_compose(struct stf_isp_dev *isp_dev,
 912                          struct v4l2_subdev_state *state,
 913                          struct v4l2_rect *rect,
 914                          enum v4l2_subdev_format_whence which)
 915 {
 916         struct v4l2_rect *crop;
 917
 918         crop = __isp_get_crop(isp_dev, state, which);
 919
 920         if (rect->width > crop->width)
 921                 rect->width = crop->width;
 922
 923         if (rect->height > crop->height)
 924                 rect->height = crop->height;
 925
 926         if (crop->width > rect->width * SCALER_RATIO_MAX)
 927                 rect->width = (crop->width + SCALER_RATIO_MAX - 1) /
 928                                                         SCALER_RATIO_MAX;
 929
 930         if (crop->height > rect->height * SCALER_RATIO_MAX)
 931                 rect->height = (crop->height + SCALER_RATIO_MAX - 1) /
 932                                                         SCALER_RATIO_MAX;
 933
 934         if (rect->width < STFCAMSS_FRAME_MIN_WIDTH)
 935                 rect->width = STFCAMSS_FRAME_MIN_WIDTH;
 936
 937         if (rect->height < STFCAMSS_FRAME_MIN_HEIGHT)
 938                 rect->height = STFCAMSS_FRAME_MIN_HEIGHT;
 939         rect->height &= ~0x1;
 940 }
 941
 942 static void isp_try_scale(struct stf_isp_dev *isp_dev,
 943                             struct v4l2_subdev_state *state,
 944                             struct v4l2_rect *rect,
 945                             enum v4l2_subdev_format_whence which)
 946 {
 947         struct v4l2_rect *compose;
 948
 949         compose = __isp_get_compose(isp_dev, state, which);
 950
 951         if (rect->width > compose->width)
 952                 rect->width = compose->width;
 953
 954         if (rect->width + rect->left > compose->width)
 955                 rect->left = compose->width - rect->width;
 956
 957         if (rect->height > compose->height)
 958                 rect->height = compose->height;
 959
 960         if (rect->height + rect->top > compose->height)
 961                 rect->top = compose->height - rect->height;
 962
 963         if (rect->width < STFCAMSS_FRAME_MIN_WIDTH) {
 964                 rect->left = 0;
 965                 rect->width = STFCAMSS_FRAME_MIN_WIDTH;
 966         }
 967
 968         if (rect->height < STFCAMSS_FRAME_MIN_HEIGHT) {
 969                 rect->top = 0;
 970                 rect->height = STFCAMSS_FRAME_MIN_HEIGHT;
 971         }
 972         rect->height &= ~0x1;
 973 }
 974
 975 static void isp_try_itiws(struct stf_isp_dev *isp_dev,
 976                             struct v4l2_subdev_state *state,
 977                             struct v4l2_rect *rect,
 978                             enum v4l2_subdev_format_whence which)
 979 {
 980         struct v4l2_rect *crop;
 981
 982         crop = __isp_get_crop(isp_dev, state, which);
 983
 984         if (rect->width > crop->width)
 985                 rect->width = crop->width;
 986
 987         if (rect->width + rect->left > crop->width)
 988                 rect->left = crop->width - rect->width;
 989
 990         if (rect->height > crop->height)
 991                 rect->height = crop->height;
 992
 993         if (rect->height + rect->top > crop->height)
 994                 rect->top = crop->height - rect->height;
 995
 996         if (rect->width < STFCAMSS_FRAME_MIN_WIDTH) {
 997                 rect->left = 0;
 998                 rect->width = STFCAMSS_FRAME_MIN_WIDTH;
 999         }
1000
1001         if (rect->height < STFCAMSS_FRAME_MIN_HEIGHT) {
1002                 rect->top = 0;
1003                 rect->height = STFCAMSS_FRAME_MIN_HEIGHT;
1004         }
1005         rect->height &= ~0x1;
1006 }
1007
1008 static int isp_get_selection(struct v4l2_subdev *sd,
1009                              struct v4l2_subdev_state *state,
1010                              struct v4l2_subdev_selection *sel)
1011 {
1012         struct stf_isp_dev *isp_dev = v4l2_get_subdevdata(sd);
1013         struct v4l2_subdev_format fmt = { 0 };
1014         struct v4l2_rect *rect;
1015         int ret;
1016
1017         switch (sel->target) {
1018         case V4L2_SEL_TGT_CROP_BOUNDS:
1019         case V4L2_SEL_TGT_CROP_DEFAULT:
1020                 fmt.pad = sel->pad;
1021                 fmt.which = sel->which;
1022                 ret = isp_get_format(sd, state, &fmt);
1023                 if (ret < 0)
1024                         return ret;
1025
1026                 sel->r.left = 0;
1027                 sel->r.top = 0;
1028                 sel->r.width = fmt.format.width;
1029                 sel->r.height = fmt.format.height;
1030                 break;
1031         case V4L2_SEL_TGT_CROP:
1032                 rect = __isp_get_crop(isp_dev, state, sel->which);
1033                 if (rect == NULL)
1034                         return -EINVAL;
1035
1036                 sel->r = *rect;
1037                 break;
1038         case V4L2_SEL_TGT_COMPOSE_BOUNDS:
1039         case V4L2_SEL_TGT_COMPOSE_DEFAULT:
1040                 if (sel->pad > STF_ISP_PAD_SRC_ITIR)
1041                         return -EINVAL;
1042                 rect = __isp_get_crop(isp_dev, state, sel->which);
1043                 if (rect == NULL)
1044                         return -EINVAL;
1045
1046                 sel->r.left = rect->left;
1047                 sel->r.top = rect->top;
1048                 sel->r.width = rect->width;
1049                 sel->r.height = rect->height;
1050                 break;
1051         case V4L2_SEL_TGT_COMPOSE:
1052                 if (sel->pad > STF_ISP_PAD_SRC_ITIR)
1053                         return -EINVAL;
1054                 if (sel->pad == STF_ISP_PAD_SRC_SS0
1055                         || sel->pad == STF_ISP_PAD_SRC_SS1) {
1056                         rect = __isp_get_scale(isp_dev, state, sel);
1057                         if (rect == NULL)
1058                                 return -EINVAL;
1059                 } else if (sel->pad == STF_ISP_PAD_SRC_ITIW
1060                         || sel->pad == STF_ISP_PAD_SRC_ITIR) {
1061                         rect = __isp_get_itiws(isp_dev, state, sel->which);
1062                         if (rect == NULL)
1063                                 return -EINVAL;
1064                 } else {
1065                         rect = __isp_get_compose(isp_dev, state, sel->which);
1066                         if (rect == NULL)
1067                                 return -EINVAL;
1068                 }
1069                 sel->r = *rect;
1070                 break;
1071         default:
1072                 return -EINVAL;
1073         }
1074
1075         st_info(ST_ISP, "%s pad = %d, left = %d, %d, %d, %d\n",
1076                         __func__, sel->pad, sel->r.left, sel->r.top, sel->r.width, sel->r.height);
1077         return 0;
1078 }
1079
1080 static int isp_set_selection(struct v4l2_subdev *sd,
1081                              struct v4l2_subdev_state *state,
1082                              struct v4l2_subdev_selection *sel)
1083 {
1084         struct stf_isp_dev *isp_dev = v4l2_get_subdevdata(sd);
1085         struct v4l2_rect *rect;
1086         int ret = 0;
1087
1088         if (sel->target == V4L2_SEL_TGT_COMPOSE &&
1089                         ((sel->pad == STF_ISP_PAD_SINK)
1090                          || (sel->pad == STF_ISP_PAD_SRC))) {
1091                 struct v4l2_subdev_format fmt = { 0 };
1092                 int i;
1093
1094                 rect = __isp_get_compose(isp_dev, state, sel->which);
1095                 if (rect == NULL)
1096                         return -EINVAL;
1097
1098                 mutex_lock(&isp_dev->stream_lock);
1099                 if (isp_dev->stream_count) {
1100                         sel->r = *rect;
1101                         mutex_unlock(&isp_dev->stream_lock);
1102                         ret = 0;
1103                         goto out;
1104                 } else {
1105                         isp_try_compose(isp_dev, state, &sel->r, sel->which);
1106                         *rect = sel->r;
1107                 }
1108                 mutex_unlock(&isp_dev->stream_lock);
1109
1110                 /* Reset source pad format width and height */
1111                 fmt.which = sel->which;
1112                 fmt.pad = STF_ISP_PAD_SRC;
1113                 ret = isp_get_format(sd, state, &fmt);
1114                 if (ret < 0)
1115                         return ret;
1116
1117                 fmt.format.width = rect->width;
1118                 fmt.format.height = rect->height;
1119                 ret = isp_set_format(sd, state, &fmt);
1120
1121                 /* Reset scale */
1122                 for (i = STF_ISP_PAD_SRC_SS0; i <= STF_ISP_PAD_SRC_ITIR; i++) {
1123                         struct v4l2_subdev_selection scale = { 0 };
1124
1125                         scale.which = sel->which;
1126                         scale.target = V4L2_SEL_TGT_COMPOSE;
1127                         scale.r = *rect;
1128                         scale.pad = i;
1129                         ret = isp_set_selection(sd, state, &scale);
1130                 }
1131         } else if (sel->target == V4L2_SEL_TGT_COMPOSE
1132                         && ((sel->pad == STF_ISP_PAD_SRC_SS0)
1133                                 || (sel->pad == STF_ISP_PAD_SRC_SS1))) {
1134                 struct v4l2_subdev_format fmt = { 0 };
1135
1136                 rect = __isp_get_scale(isp_dev, state, sel);
1137                 if (rect == NULL)
1138                         return -EINVAL;
1139
1140                 mutex_lock(&isp_dev->stream_lock);
1141                 if (isp_dev->stream_count) {
1142                         sel->r = *rect;
1143                         mutex_unlock(&isp_dev->stream_lock);
1144                         ret = 0;
1145                         goto out;
1146                 } else {
1147                         isp_try_scale(isp_dev, state, &sel->r, sel->which);
1148                         *rect = sel->r;
1149                 }
1150                 mutex_unlock(&isp_dev->stream_lock);
1151
1152                 /* Reset source pad format width and height */
1153                 fmt.which = sel->which;
1154                 fmt.pad = sel->pad;
1155                 ret = isp_get_format(sd, state, &fmt);
1156                 if (ret < 0)
1157                         return ret;
1158
1159                 fmt.format.width = rect->width;
1160                 fmt.format.height = rect->height;
1161                 ret = isp_set_format(sd, state, &fmt);
1162         } else if (sel->target == V4L2_SEL_TGT_COMPOSE
1163                         && ((sel->pad == STF_ISP_PAD_SRC_ITIW)
1164                                 || (sel->pad == STF_ISP_PAD_SRC_ITIR))) {
1165                 struct v4l2_subdev_format fmt = { 0 };
1166
1167                 rect = __isp_get_itiws(isp_dev, state, sel->which);
1168                 if (rect == NULL)
1169                         return -EINVAL;
1170
1171                 mutex_lock(&isp_dev->stream_lock);
1172                 if (isp_dev->stream_count) {
1173                         sel->r = *rect;
1174                         mutex_unlock(&isp_dev->stream_lock);
1175                         ret = 0;
1176                         goto out;
1177                 } else {
1178                         isp_try_itiws(isp_dev, state, &sel->r, sel->which);
1179                         *rect = sel->r;
1180                 }
1181                 mutex_unlock(&isp_dev->stream_lock);
1182
1183                 /* Reset source pad format width and height */
1184                 fmt.which = sel->which;
1185                 fmt.pad = sel->pad;
1186                 ret = isp_get_format(sd, state, &fmt);
1187                 if (ret < 0)
1188                         return ret;
1189
1190                 fmt.format.width = rect->width;
1191                 fmt.format.height = rect->height;
1192                 ret = isp_set_format(sd, state, &fmt);
1193         } else if (sel->target == V4L2_SEL_TGT_CROP) {
1194                 struct v4l2_subdev_selection compose = { 0 };
1195                 int i;
1196
1197                 rect = __isp_get_crop(isp_dev, state, sel->which);
1198                 if (rect == NULL)
1199                         return -EINVAL;
1200
1201                 mutex_lock(&isp_dev->stream_lock);
1202                 if (isp_dev->stream_count) {
1203                         sel->r = *rect;
1204                         mutex_unlock(&isp_dev->stream_lock);
1205                         ret = 0;
1206                         goto out;
1207                 } else {
1208                         isp_try_crop(isp_dev, state, &sel->r, sel->which);
1209                         *rect = sel->r;
1210                 }
1211                 mutex_unlock(&isp_dev->stream_lock);
1212
1213                 /* Reset source compose selection */
1214                 compose.which = sel->which;
1215                 compose.target = V4L2_SEL_TGT_COMPOSE;
1216                 compose.r.width = rect->width;
1217                 compose.r.height = rect->height;
1218                 compose.pad = STF_ISP_PAD_SINK;
1219                 ret = isp_set_selection(sd, state, &compose);
1220
1221                 /* Reset source pad format width and height */
1222                 for (i = STF_ISP_PAD_SRC_RAW; i < STF_ISP_PAD_MAX; i++) {
1223                         struct v4l2_subdev_format fmt = { 0 };
1224
1225                         fmt.which = sel->which;
1226                         fmt.pad = i;
1227                         ret = isp_get_format(sd, state, &fmt);
1228                         if (ret < 0)
1229                                 return ret;
1230
1231                         fmt.format.width = rect->width;
1232                         fmt.format.height = rect->height;
1233                         ret = isp_set_format(sd, state, &fmt);
1234                 }
1235         } else {
1236                 ret = -EINVAL;
1237         }
1238
1239         st_info(ST_ISP, "%s pad = %d, left = %d, %d, %d, %d\n",
1240                         __func__, sel->pad, sel->r.left, sel->r.top, sel->r.width, sel->r.height);
1241 out:
1242         return ret;
1243 }
1244
1245 static int isp_init_formats(struct v4l2_subdev *sd,
1246                         struct v4l2_subdev_fh *fh)
1247 {
1248         struct v4l2_subdev_format format = {
1249                 .pad = STF_ISP_PAD_SINK,
1250                 .which = fh ? V4L2_SUBDEV_FORMAT_TRY :
1251                                 V4L2_SUBDEV_FORMAT_ACTIVE,
1252                 .format = {
1253                         .code = MEDIA_BUS_FMT_RGB565_2X8_LE,
1254                         .width = 1920,
1255                         .height = 1080
1256                 }
1257         };
1258
1259         return isp_set_format(sd, fh ? fh->state : NULL, &format);
1260 }
1261
1262 static int isp_link_setup(struct media_entity *entity,
1263                         const struct media_pad *local,
1264                         const struct media_pad *remote, u32 flags)
1265 {
1266         if (flags & MEDIA_LNK_FL_ENABLED)
1267                 if (media_entity_remote_pad(local))
1268                         return -EBUSY;
1269         return 0;
1270 }
1271
1272 static int stf_isp_load_setfile(struct stf_isp_dev *isp_dev, char *file_name)
1273 {
1274         struct device *dev = isp_dev->stfcamss->dev;
1275         const struct firmware *fw;
1276         u8 *buf = NULL;
1277         int *regval_num;
1278         int ret;
1279
1280         st_debug(ST_ISP, "%s, file_name %s\n", __func__, file_name);
1281         ret = request_firmware(&fw, file_name, dev);
1282         if (ret < 0) {
1283                 st_err(ST_ISP, "firmware request failed (%d)\n", ret);
1284                 return ret;
1285         }
1286         buf = devm_kzalloc(dev, fw->size, GFP_KERNEL);
1287         if (!buf)
1288                 return -ENOMEM;
1289         memcpy(buf, fw->data, fw->size);
1290
1291         mutex_lock(&isp_dev->setfile_lock);
1292         if (isp_dev->setfile.state == 1)
1293                 devm_kfree(dev, isp_dev->setfile.data);
1294         isp_dev->setfile.data = buf;
1295         isp_dev->setfile.size = fw->size;
1296         isp_dev->setfile.state = 1;
1297         regval_num = (int *)&buf[fw->size - sizeof(unsigned int)];
1298         isp_dev->setfile.settings.regval_num = *regval_num;
1299         isp_dev->setfile.settings.regval = (struct regval_t *)buf;
1300         mutex_unlock(&isp_dev->setfile_lock);
1301
1302         st_debug(ST_ISP, "stf_isp setfile loaded size: %zu B, reg_nul: %d\n",
1303                         fw->size, isp_dev->setfile.settings.regval_num);
1304
1305         release_firmware(fw);
1306         return ret;
1307 }
1308
1309 static long stf_isp_ioctl(struct v4l2_subdev *sd, unsigned int cmd, void *arg)
1310 {
1311         struct stf_isp_dev *isp_dev = v4l2_get_subdevdata(sd);
1312         struct device *dev = isp_dev->stfcamss->dev;
1313         int ret = -ENOIOCTLCMD;
1314
1315         switch (cmd) {
1316         case VIDIOC_STFISP_LOAD_FW: {
1317                 struct stfisp_fw_info *fw_info = arg;
1318
1319                 if (IS_ERR(fw_info)) {
1320                         st_err(ST_ISP, "fw_info failed, params invaild\n");
1321                         return -EINVAL;
1322                 }
1323
1324                 ret = stf_isp_load_setfile(isp_dev, fw_info->filename);
1325                 break;
1326         }
1327         case VIDIOC_STF_DMABUF_ALLOC:
1328         case VIDIOC_STF_DMABUF_FREE:
1329                 ret = stf_dmabuf_ioctl(dev, cmd, arg);
1330                 break;
1331         case VIDIOC_STFISP_GET_REG:
1332                 ret = isp_dev->hw_ops->isp_reg_read(isp_dev, arg);
1333                 break;
1334         case VIDIOC_STFISP_SET_REG:
1335                 ret = isp_dev->hw_ops->isp_reg_write(isp_dev, arg);
1336                 break;
1337         case VIDIOC_STFISP_SHADOW_LOCK:
1338                 if (atomic_add_unless(&isp_dev->shadow_count, 1, 1))
1339                         ret = 0;
1340                 else
1341                         ret = -EBUSY;
1342                 st_debug(ST_ISP, "%s, %d, ret = %d\n", __func__, __LINE__, ret);
1343                 break;
1344         case VIDIOC_STFISP_SHADOW_UNLOCK:
1345                 if (atomic_dec_if_positive(&isp_dev->shadow_count) < 0)
1346                         ret = -EINVAL;
1347                 else
1348                         ret = 0;
1349                 st_debug(ST_ISP, "%s, %d, ret = %d\n", __func__, __LINE__, ret);
1350                 break;
1351         case VIDIOC_STFISP_SHADOW_UNLOCK_N_TRIGGER:
1352                 {
1353                         isp_dev->hw_ops->isp_shadow_trigger(isp_dev);
1354                         if (atomic_dec_if_positive(&isp_dev->shadow_count) < 0)
1355                                 ret = -EINVAL;
1356                         else
1357                                 ret = 0;
1358                         st_debug(ST_ISP, "%s, %d, ret = %d\n", __func__, __LINE__, ret);
1359                 }
1360                 break;
1361         case VIDIOC_STFISP_SET_USER_CONFIG_ISP:
1362                 st_debug(ST_ISP, "%s, %d set user_config_isp\n", __func__, __LINE__);
1363                 user_config_isp = 1;
1364                 break;
1365         default:
1366                 break;
1367         }
1368         return ret;
1369 }
1370
1371 int isp_close(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
1372 {
1373         struct stf_isp_dev *isp_dev = v4l2_get_subdevdata(sd);
1374
1375         st_debug(ST_ISP, "%s, %d\n", __func__, __LINE__);
1376         while (atomic_dec_if_positive(&isp_dev->shadow_count) > 0)
1377                 st_warn(ST_ISP, "user not unlocked the shadow lock, driver unlock it!\n");
1378
1379         return 0;
1380 }
1381
1382 static int stf_isp_subscribe_event(struct v4l2_subdev *sd,
1383                                    struct v4l2_fh *fh,
1384                                    struct v4l2_event_subscription *sub)
1385 {
1386         switch (sub->type) {
1387         case V4L2_EVENT_SOURCE_CHANGE:
1388                 return v4l2_src_change_event_subdev_subscribe(sd, fh, sub);
1389         case V4L2_EVENT_CTRL:
1390                 return v4l2_ctrl_subdev_subscribe_event(sd, fh, sub);
1391         default:
1392                 st_debug(ST_ISP, "unspport subscribe_event\n");
1393                 return -EINVAL;
1394         }
1395 }
1396
1397 static const struct v4l2_subdev_core_ops isp_core_ops = {
1398         .s_power = isp_set_power,
1399         .ioctl = stf_isp_ioctl,
1400         .log_status = v4l2_ctrl_subdev_log_status,
1401         // .subscribe_event = v4l2_ctrl_subdev_subscribe_event,
1402         .subscribe_event = stf_isp_subscribe_event,
1403         .unsubscribe_event = v4l2_event_subdev_unsubscribe,
1404 };
1405
1406 static const struct v4l2_subdev_video_ops isp_video_ops = {
1407         .s_stream = isp_set_stream,
1408 };
1409
1410 static const struct v4l2_subdev_pad_ops isp_pad_ops = {
1411         .enum_mbus_code = isp_enum_mbus_code,
1412         .enum_frame_size = isp_enum_frame_size,
1413         .get_fmt = isp_get_format,
1414         .set_fmt = isp_set_format,
1415         .get_selection = isp_get_selection,
1416         .set_selection = isp_set_selection,
1417 };
1418
1419 static const struct v4l2_subdev_ops isp_v4l2_ops = {
1420         .core = &isp_core_ops,
1421         .video = &isp_video_ops,
1422         .pad = &isp_pad_ops,
1423 };
1424
1425 static const struct v4l2_subdev_internal_ops isp_v4l2_internal_ops = {
1426         .open = isp_init_formats,
1427         .close = isp_close,
1428 };
1429
1430 static const struct media_entity_operations isp_media_ops = {
1431         .link_setup = isp_link_setup,
1432         .link_validate = v4l2_subdev_link_validate,
1433 };
1434
1435 int stf_isp_register(struct stf_isp_dev *isp_dev,
1436                 struct v4l2_device *v4l2_dev)
1437 {
1438         struct v4l2_subdev *sd = &isp_dev->subdev;
1439         struct media_pad *pads = isp_dev->pads;
1440         int ret;
1441
1442         v4l2_subdev_init(sd, &isp_v4l2_ops);
1443         sd->internal_ops = &isp_v4l2_internal_ops;
1444         sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE | V4L2_SUBDEV_FL_HAS_EVENTS;
1445         snprintf(sd->name, ARRAY_SIZE(sd->name), "%s%d",
1446                 STF_ISP_NAME, 0);
1447         v4l2_set_subdevdata(sd, isp_dev);
1448
1449         ret = isp_init_formats(sd, NULL);
1450         if (ret < 0) {
1451                 st_err(ST_ISP, "Failed to init format: %d\n", ret);
1452                 return ret;
1453         }
1454
1455         pads[STF_ISP_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
1456         pads[STF_ISP_PAD_SRC].flags = MEDIA_PAD_FL_SOURCE;
1457         pads[STF_ISP_PAD_SRC_SS0].flags = MEDIA_PAD_FL_SOURCE;
1458         pads[STF_ISP_PAD_SRC_SS1].flags = MEDIA_PAD_FL_SOURCE;
1459         pads[STF_ISP_PAD_SRC_ITIW].flags = MEDIA_PAD_FL_SOURCE;
1460         pads[STF_ISP_PAD_SRC_ITIR].flags = MEDIA_PAD_FL_SOURCE;
1461         pads[STF_ISP_PAD_SRC_RAW].flags = MEDIA_PAD_FL_SOURCE;
1462         pads[STF_ISP_PAD_SRC_SCD_Y].flags = MEDIA_PAD_FL_SOURCE;
1463
1464         sd->entity.function = MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER;
1465         sd->entity.ops = &isp_media_ops;
1466         ret = media_entity_pads_init(&sd->entity, STF_ISP_PAD_MAX, pads);
1467         if (ret < 0) {
1468                 st_err(ST_ISP, "Failed to init media entity: %d\n", ret);
1469                 return ret;
1470         }
1471
1472         ret = isp_init_controls(isp_dev);
1473         if (ret)
1474                 goto err_sreg;
1475
1476         ret = v4l2_device_register_subdev(v4l2_dev, sd);
1477         if (ret < 0) {
1478                 st_err(ST_ISP, "Failed to register subdev: %d\n", ret);
1479                 goto free_ctrls;
1480         }
1481
1482         return 0;
1483
1484 free_ctrls:
1485         v4l2_ctrl_handler_free(&isp_dev->ctrls.handler);
1486 err_sreg:
1487         media_entity_cleanup(&sd->entity);
1488         return ret;
1489 }
1490
1491 int stf_isp_unregister(struct stf_isp_dev *isp_dev)
1492 {
1493         v4l2_device_unregister_subdev(&isp_dev->subdev);
1494         media_entity_cleanup(&isp_dev->subdev.entity);
1495         v4l2_ctrl_handler_free(&isp_dev->ctrls.handler);
1496         mutex_destroy(&isp_dev->stream_lock);
1497         mutex_destroy(&isp_dev->power_lock);
1498         mutex_destroy(&isp_dev->setfile_lock);
1499         return 0;
1500 }