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, "isp_match_format_get_index = %d\n", i);
 624
 625         if (i >= formats->nfmts &&
 626                 (pad == STF_ISP_PAD_SRC_RAW || pad == STF_ISP_PAD_SRC_SCD_Y)) {
 627                 int sensor_idx;
 628
 629                 sensor_idx = isp_match_sensor_format_get_index(isp_dev);
 630                 if (sensor_idx)
 631                         i = sensor_idx;
 632         }
 633
 634         if (pad != STF_ISP_PAD_SINK)
 635                 *fmt = *__isp_get_format(isp_dev, state, STF_ISP_PAD_SINK, which);
 636
 637         if (i >= formats->nfmts) {
 638                 fmt->code = formats->fmts[0].code;
 639                 bpp = formats->fmts[0].bpp;
 640                 st_info(ST_ISP, "Use default index 0 format = 0x%x\n", fmt->code);
 641         } else {
 642                 // sink format and raw format must one by one
 643                 if (pad == STF_ISP_PAD_SRC_RAW || pad == STF_ISP_PAD_SRC_SCD_Y) {
 644                         fmt->code = formats->fmts[i].code;
 645                         bpp = formats->fmts[i].bpp;
 646                         st_info(ST_ISP, "Use mapping format from sink index %d = 0x%x\n",
 647                                         i, fmt->code);
 648                 } else {
 649                         fmt->code = code;
 650                         bpp = formats->fmts[i].bpp;
 651                         st_info(ST_ISP, "Use input format = 0x%x\n", fmt->code);
 652                 }
 653         }
 654
 655         switch (pad) {
 656         case STF_ISP_PAD_SINK:
 657                 break;
 658         case STF_ISP_PAD_SRC:
 659                 isp_dev->rect[ISP_COMPOSE].bpp = bpp;
 660                 break;
 661         case STF_ISP_PAD_SRC_SS0:
 662                 isp_dev->rect[ISP_SCALE_SS0].bpp = bpp;
 663                 break;
 664         case STF_ISP_PAD_SRC_SS1:
 665                 isp_dev->rect[ISP_SCALE_SS1].bpp = bpp;
 666                 break;
 667         case STF_ISP_PAD_SRC_ITIW:
 668         case STF_ISP_PAD_SRC_ITIR:
 669                 isp_dev->rect[ISP_ITIWS].bpp = bpp;
 670                 break;
 671         case STF_ISP_PAD_SRC_RAW:
 672                 isp_dev->rect[ISP_CROP].bpp = bpp;
 673                 break;
 674         case STF_ISP_PAD_SRC_SCD_Y:
 675                 break;
 676         }
 677 }
 678
 679 static int isp_enum_mbus_code(struct v4l2_subdev *sd,
 680                         struct v4l2_subdev_state *state,
 681                         struct v4l2_subdev_mbus_code_enum *code)
 682 {
 683         struct stf_isp_dev *isp_dev = v4l2_get_subdevdata(sd);
 684         const struct isp_format_table *formats;
 685
 686         if (code->index >= isp_dev->nformats)
 687                 return -EINVAL;
 688         if (code->pad == STF_ISP_PAD_SINK) {
 689                 formats = &isp_dev->formats[SINK_FORMATS_INDEX];
 690                 code->code = formats->fmts[code->index].code;
 691         } else {
 692                 struct v4l2_mbus_framefmt *sink_fmt;
 693
 694                 sink_fmt = __isp_get_format(isp_dev, state, STF_ISP_PAD_SINK,
 695                                         code->which);
 696
 697                 code->code = sink_fmt->code;
 698                 if (!code->code)
 699                         return -EINVAL;
 700         }
 701         code->flags = 0;
 702
 703         return 0;
 704 }
 705
 706 static int isp_enum_frame_size(struct v4l2_subdev *sd,
 707                                 struct v4l2_subdev_state *state,
 708                                 struct v4l2_subdev_frame_size_enum *fse)
 709 {
 710         struct stf_isp_dev *isp_dev = v4l2_get_subdevdata(sd);
 711         struct v4l2_mbus_framefmt format;
 712
 713         if (fse->index != 0)
 714                 return -EINVAL;
 715
 716         format.code = fse->code;
 717         format.width = 1;
 718         format.height = 1;
 719         isp_try_format(isp_dev, state, fse->pad, &format, fse->which);
 720         fse->min_width = format.width;
 721         fse->min_height = format.height;
 722
 723         if (format.code != fse->code)
 724                 return -EINVAL;
 725
 726         format.code = fse->code;
 727         format.width = -1;
 728         format.height = -1;
 729         isp_try_format(isp_dev, state, fse->pad, &format, fse->which);
 730         fse->max_width = format.width;
 731         fse->max_height = format.height;
 732
 733         return 0;
 734 }
 735
 736 static int isp_get_format(struct v4l2_subdev *sd,
 737                         struct v4l2_subdev_state *state,
 738                         struct v4l2_subdev_format *fmt)
 739 {
 740         struct stf_isp_dev *isp_dev = v4l2_get_subdevdata(sd);
 741         struct v4l2_mbus_framefmt *format;
 742
 743         format = __isp_get_format(isp_dev, state, fmt->pad, fmt->which);
 744         if (format == NULL)
 745                 return -EINVAL;
 746
 747         fmt->format = *format;
 748
 749         return 0;
 750 }
 751
 752 static int isp_set_format(struct v4l2_subdev *sd,
 753                         struct v4l2_subdev_state *state,
 754                         struct v4l2_subdev_format *fmt)
 755 {
 756         struct stf_isp_dev *isp_dev = v4l2_get_subdevdata(sd);
 757         struct v4l2_mbus_framefmt *format;
 758         struct v4l2_subdev_selection sel = { 0 };
 759         struct v4l2_rect *rect = NULL;
 760         int ret;
 761
 762         format = __isp_get_format(isp_dev, state, fmt->pad, fmt->which);
 763         if (format == NULL)
 764                 return -EINVAL;
 765
 766         mutex_lock(&isp_dev->stream_lock);
 767         if (isp_dev->stream_count) {
 768                 fmt->format = *format;
 769                 if (fmt->reserved[0] != 0) {
 770                         sel.which = fmt->which;
 771                         sel.pad = fmt->reserved[0];
 772
 773                         switch (fmt->reserved[0]) {
 774                         case STF_ISP_PAD_SRC:
 775                                 rect = __isp_get_compose(isp_dev, state, fmt->which);
 776                                 break;
 777                         case STF_ISP_PAD_SRC_SS0:
 778                         case STF_ISP_PAD_SRC_SS1:
 779                                 rect = __isp_get_scale(isp_dev, state, &sel);
 780                                 break;
 781                         case STF_ISP_PAD_SRC_ITIW:
 782                         case STF_ISP_PAD_SRC_ITIR:
 783                                 rect = __isp_get_itiws(isp_dev, state, fmt->which);
 784                                 break;
 785                         case STF_ISP_PAD_SRC_RAW:
 786                         case STF_ISP_PAD_SRC_SCD_Y:
 787                                 rect = __isp_get_crop(isp_dev, state, fmt->which);
 788                                 break;
 789                         default:
 790                                 break;
 791                         }
 792                         if (rect != NULL) {
 793                                 fmt->format.width = rect->width;
 794                                 fmt->format.height = rect->height;
 795                         }
 796                 }
 797                 mutex_unlock(&isp_dev->stream_lock);
 798                 goto out;
 799         } else {
 800                 isp_try_format(isp_dev, state, fmt->pad, &fmt->format, fmt->which);
 801                 *format = fmt->format;
 802         }
 803         mutex_unlock(&isp_dev->stream_lock);
 804
 805         /* Propagate the format from sink to source */
 806         if (fmt->pad == STF_ISP_PAD_SINK) {
 807                 /* Reset sink pad compose selection */
 808                 sel.which = fmt->which;
 809                 sel.pad = STF_ISP_PAD_SINK;
 810                 sel.target = V4L2_SEL_TGT_CROP;
 811                 sel.r.width = fmt->format.width;
 812                 sel.r.height = fmt->format.height;
 813                 ret = isp_set_selection(sd, state, &sel);
 814                 if (ret < 0)
 815                         return ret;
 816         }
 817
 818 out:
 819         return 0;
 820 }
 821
 822 static struct v4l2_rect *
 823 __isp_get_compose(struct stf_isp_dev *isp_dev,
 824                   struct v4l2_subdev_state *state,
 825                   enum v4l2_subdev_format_whence which)
 826 {
 827         if (which == V4L2_SUBDEV_FORMAT_TRY)
 828                 return v4l2_subdev_get_try_compose(&isp_dev->subdev, state,
 829                                                    STF_ISP_PAD_SINK);
 830
 831
 832         return &isp_dev->rect[ISP_COMPOSE].rect;
 833 }
 834
 835 static struct v4l2_rect *
 836 __isp_get_crop(struct stf_isp_dev *isp_dev,
 837                 struct v4l2_subdev_state *state,
 838                 enum v4l2_subdev_format_whence which)
 839 {
 840         if (which == V4L2_SUBDEV_FORMAT_TRY)
 841                 return v4l2_subdev_get_try_crop(&isp_dev->subdev, state,
 842                                                 STF_ISP_PAD_SINK);
 843
 844         return &isp_dev->rect[ISP_CROP].rect;
 845 }
 846
 847 static struct v4l2_rect *
 848 __isp_get_scale(struct stf_isp_dev *isp_dev,
 849                 struct v4l2_subdev_state *state,
 850                 struct v4l2_subdev_selection *sel)
 851 {
 852         int pad;
 853
 854         if (sel->which == V4L2_SUBDEV_FORMAT_TRY)
 855                 return v4l2_subdev_get_try_compose(&isp_dev->subdev, state,
 856                                                 STF_ISP_PAD_SINK);
 857         if (sel->pad != STF_ISP_PAD_SRC_SS0 && sel->pad != STF_ISP_PAD_SRC_SS1)
 858                 return NULL;
 859
 860         pad = sel->pad == STF_ISP_PAD_SRC_SS0 ? ISP_SCALE_SS0 : ISP_SCALE_SS1;
 861         return &isp_dev->rect[pad].rect;
 862 }
 863
 864 static struct v4l2_rect *
 865 __isp_get_itiws(struct stf_isp_dev *isp_dev,
 866                 struct v4l2_subdev_state *state,
 867                 enum v4l2_subdev_format_whence which)
 868 {
 869         if (which == V4L2_SUBDEV_FORMAT_TRY)
 870                 return v4l2_subdev_get_try_crop(&isp_dev->subdev, state, STF_ISP_PAD_SINK);
 871
 872         return &isp_dev->rect[ISP_ITIWS].rect;
 873 }
 874
 875 static void isp_try_crop(struct stf_isp_dev *isp_dev,
 876                             struct v4l2_subdev_state *state,
 877                             struct v4l2_rect *rect,
 878                             enum v4l2_subdev_format_whence which)
 879 {
 880         struct v4l2_mbus_framefmt *fmt;
 881
 882         fmt = __isp_get_format(isp_dev, state, STF_ISP_PAD_SINK, which);
 883
 884         if (rect->width > fmt->width)
 885                 rect->width = fmt->width;
 886
 887         if (rect->width + rect->left > fmt->width)
 888                 rect->left = fmt->width - rect->width;
 889
 890         if (rect->height > fmt->height)
 891                 rect->height = fmt->height;
 892
 893         if (rect->height + rect->top > fmt->height)
 894                 rect->top = fmt->height - rect->height;
 895
 896         if (rect->width < STFCAMSS_FRAME_MIN_WIDTH) {
 897                 rect->left = 0;
 898                 rect->width = STFCAMSS_FRAME_MIN_WIDTH;
 899         }
 900
 901         if (rect->height < STFCAMSS_FRAME_MIN_HEIGHT) {
 902                 rect->top = 0;
 903                 rect->height = STFCAMSS_FRAME_MIN_HEIGHT;
 904         }
 905         rect->height &= ~0x1;
 906 }
 907
 908 static void isp_try_compose(struct stf_isp_dev *isp_dev,
 909                          struct v4l2_subdev_state *state,
 910                          struct v4l2_rect *rect,
 911                          enum v4l2_subdev_format_whence which)
 912 {
 913         struct v4l2_rect *crop;
 914
 915         crop = __isp_get_crop(isp_dev, state, which);
 916
 917         if (rect->width > crop->width)
 918                 rect->width = crop->width;
 919
 920         if (rect->height > crop->height)
 921                 rect->height = crop->height;
 922
 923         if (crop->width > rect->width * SCALER_RATIO_MAX)
 924                 rect->width = (crop->width + SCALER_RATIO_MAX - 1) /
 925                                                         SCALER_RATIO_MAX;
 926
 927         if (crop->height > rect->height * SCALER_RATIO_MAX)
 928                 rect->height = (crop->height + SCALER_RATIO_MAX - 1) /
 929                                                         SCALER_RATIO_MAX;
 930
 931         if (rect->width < STFCAMSS_FRAME_MIN_WIDTH)
 932                 rect->width = STFCAMSS_FRAME_MIN_WIDTH;
 933
 934         if (rect->height < STFCAMSS_FRAME_MIN_HEIGHT)
 935                 rect->height = STFCAMSS_FRAME_MIN_HEIGHT;
 936         rect->height &= ~0x1;
 937 }
 938
 939 static void isp_try_scale(struct stf_isp_dev *isp_dev,
 940                             struct v4l2_subdev_state *state,
 941                             struct v4l2_rect *rect,
 942                             enum v4l2_subdev_format_whence which)
 943 {
 944         struct v4l2_rect *compose;
 945
 946         compose = __isp_get_compose(isp_dev, state, which);
 947
 948         if (rect->width > compose->width)
 949                 rect->width = compose->width;
 950
 951         if (rect->width + rect->left > compose->width)
 952                 rect->left = compose->width - rect->width;
 953
 954         if (rect->height > compose->height)
 955                 rect->height = compose->height;
 956
 957         if (rect->height + rect->top > compose->height)
 958                 rect->top = compose->height - rect->height;
 959
 960         if (rect->width < STFCAMSS_FRAME_MIN_WIDTH) {
 961                 rect->left = 0;
 962                 rect->width = STFCAMSS_FRAME_MIN_WIDTH;
 963         }
 964
 965         if (rect->height < STFCAMSS_FRAME_MIN_HEIGHT) {
 966                 rect->top = 0;
 967                 rect->height = STFCAMSS_FRAME_MIN_HEIGHT;
 968         }
 969         rect->height &= ~0x1;
 970 }
 971
 972 static void isp_try_itiws(struct stf_isp_dev *isp_dev,
 973                             struct v4l2_subdev_state *state,
 974                             struct v4l2_rect *rect,
 975                             enum v4l2_subdev_format_whence which)
 976 {
 977         struct v4l2_rect *crop;
 978
 979         crop = __isp_get_crop(isp_dev, state, which);
 980
 981         if (rect->width > crop->width)
 982                 rect->width = crop->width;
 983
 984         if (rect->width + rect->left > crop->width)
 985                 rect->left = crop->width - rect->width;
 986
 987         if (rect->height > crop->height)
 988                 rect->height = crop->height;
 989
 990         if (rect->height + rect->top > crop->height)
 991                 rect->top = crop->height - rect->height;
 992
 993         if (rect->width < STFCAMSS_FRAME_MIN_WIDTH) {
 994                 rect->left = 0;
 995                 rect->width = STFCAMSS_FRAME_MIN_WIDTH;
 996         }
 997
 998         if (rect->height < STFCAMSS_FRAME_MIN_HEIGHT) {
 999                 rect->top = 0;
1000                 rect->height = STFCAMSS_FRAME_MIN_HEIGHT;
1001         }
1002         rect->height &= ~0x1;
1003 }
1004
1005 static int isp_get_selection(struct v4l2_subdev *sd,
1006                              struct v4l2_subdev_state *state,
1007                              struct v4l2_subdev_selection *sel)
1008 {
1009         struct stf_isp_dev *isp_dev = v4l2_get_subdevdata(sd);
1010         struct v4l2_subdev_format fmt = { 0 };
1011         struct v4l2_rect *rect;
1012         int ret;
1013
1014         switch (sel->target) {
1015         case V4L2_SEL_TGT_CROP_BOUNDS:
1016         case V4L2_SEL_TGT_CROP_DEFAULT:
1017                 fmt.pad = sel->pad;
1018                 fmt.which = sel->which;
1019                 ret = isp_get_format(sd, state, &fmt);
1020                 if (ret < 0)
1021                         return ret;
1022
1023                 sel->r.left = 0;
1024                 sel->r.top = 0;
1025                 sel->r.width = fmt.format.width;
1026                 sel->r.height = fmt.format.height;
1027                 break;
1028         case V4L2_SEL_TGT_CROP:
1029                 rect = __isp_get_crop(isp_dev, state, sel->which);
1030                 if (rect == NULL)
1031                         return -EINVAL;
1032
1033                 sel->r = *rect;
1034                 break;
1035         case V4L2_SEL_TGT_COMPOSE_BOUNDS:
1036         case V4L2_SEL_TGT_COMPOSE_DEFAULT:
1037                 if (sel->pad > STF_ISP_PAD_SRC_ITIR)
1038                         return -EINVAL;
1039                 rect = __isp_get_crop(isp_dev, state, sel->which);
1040                 if (rect == NULL)
1041                         return -EINVAL;
1042
1043                 sel->r.left = rect->left;
1044                 sel->r.top = rect->top;
1045                 sel->r.width = rect->width;
1046                 sel->r.height = rect->height;
1047                 break;
1048         case V4L2_SEL_TGT_COMPOSE:
1049                 if (sel->pad > STF_ISP_PAD_SRC_ITIR)
1050                         return -EINVAL;
1051                 if (sel->pad == STF_ISP_PAD_SRC_SS0
1052                         || sel->pad == STF_ISP_PAD_SRC_SS1) {
1053                         rect = __isp_get_scale(isp_dev, state, sel);
1054                         if (rect == NULL)
1055                                 return -EINVAL;
1056                 } else if (sel->pad == STF_ISP_PAD_SRC_ITIW
1057                         || sel->pad == STF_ISP_PAD_SRC_ITIR) {
1058                         rect = __isp_get_itiws(isp_dev, state, sel->which);
1059                         if (rect == NULL)
1060                                 return -EINVAL;
1061                 } else {
1062                         rect = __isp_get_compose(isp_dev, state, sel->which);
1063                         if (rect == NULL)
1064                                 return -EINVAL;
1065                 }
1066                 sel->r = *rect;
1067                 break;
1068         default:
1069                 return -EINVAL;
1070         }
1071
1072         st_info(ST_ISP, "%s pad = %d, left = %d, %d, %d, %d\n",
1073                         __func__, sel->pad, sel->r.left, sel->r.top, sel->r.width, sel->r.height);
1074         return 0;
1075 }
1076
1077 static int isp_set_selection(struct v4l2_subdev *sd,
1078                              struct v4l2_subdev_state *state,
1079                              struct v4l2_subdev_selection *sel)
1080 {
1081         struct stf_isp_dev *isp_dev = v4l2_get_subdevdata(sd);
1082         struct v4l2_rect *rect;
1083         int ret = 0;
1084
1085         if (sel->target == V4L2_SEL_TGT_COMPOSE &&
1086                         ((sel->pad == STF_ISP_PAD_SINK)
1087                          || (sel->pad == STF_ISP_PAD_SRC))) {
1088                 struct v4l2_subdev_format fmt = { 0 };
1089                 int i;
1090
1091                 rect = __isp_get_compose(isp_dev, state, sel->which);
1092                 if (rect == NULL)
1093                         return -EINVAL;
1094
1095                 mutex_lock(&isp_dev->stream_lock);
1096                 if (isp_dev->stream_count) {
1097                         sel->r = *rect;
1098                         mutex_unlock(&isp_dev->stream_lock);
1099                         ret = 0;
1100                         goto out;
1101                 } else {
1102                         isp_try_compose(isp_dev, state, &sel->r, sel->which);
1103                         *rect = sel->r;
1104                 }
1105                 mutex_unlock(&isp_dev->stream_lock);
1106
1107                 /* Reset source pad format width and height */
1108                 fmt.which = sel->which;
1109                 fmt.pad = STF_ISP_PAD_SRC;
1110                 ret = isp_get_format(sd, state, &fmt);
1111                 if (ret < 0)
1112                         return ret;
1113
1114                 fmt.format.width = rect->width;
1115                 fmt.format.height = rect->height;
1116                 ret = isp_set_format(sd, state, &fmt);
1117
1118                 /* Reset scale */
1119                 for (i = STF_ISP_PAD_SRC_SS0; i <= STF_ISP_PAD_SRC_ITIR; i++) {
1120                         struct v4l2_subdev_selection scale = { 0 };
1121
1122                         scale.which = sel->which;
1123                         scale.target = V4L2_SEL_TGT_COMPOSE;
1124                         scale.r = *rect;
1125                         scale.pad = i;
1126                         ret = isp_set_selection(sd, state, &scale);
1127                 }
1128         } else if (sel->target == V4L2_SEL_TGT_COMPOSE
1129                         && ((sel->pad == STF_ISP_PAD_SRC_SS0)
1130                                 || (sel->pad == STF_ISP_PAD_SRC_SS1))) {
1131                 struct v4l2_subdev_format fmt = { 0 };
1132
1133                 rect = __isp_get_scale(isp_dev, state, sel);
1134                 if (rect == NULL)
1135                         return -EINVAL;
1136
1137                 mutex_lock(&isp_dev->stream_lock);
1138                 if (isp_dev->stream_count) {
1139                         sel->r = *rect;
1140                         mutex_unlock(&isp_dev->stream_lock);
1141                         ret = 0;
1142                         goto out;
1143                 } else {
1144                         isp_try_scale(isp_dev, state, &sel->r, sel->which);
1145                         *rect = sel->r;
1146                 }
1147                 mutex_unlock(&isp_dev->stream_lock);
1148
1149                 /* Reset source pad format width and height */
1150                 fmt.which = sel->which;
1151                 fmt.pad = sel->pad;
1152                 ret = isp_get_format(sd, state, &fmt);
1153                 if (ret < 0)
1154                         return ret;
1155
1156                 fmt.format.width = rect->width;
1157                 fmt.format.height = rect->height;
1158                 ret = isp_set_format(sd, state, &fmt);
1159         } else if (sel->target == V4L2_SEL_TGT_COMPOSE
1160                         && ((sel->pad == STF_ISP_PAD_SRC_ITIW)
1161                                 || (sel->pad == STF_ISP_PAD_SRC_ITIR))) {
1162                 struct v4l2_subdev_format fmt = { 0 };
1163
1164                 rect = __isp_get_itiws(isp_dev, state, sel->which);
1165                 if (rect == NULL)
1166                         return -EINVAL;
1167
1168                 mutex_lock(&isp_dev->stream_lock);
1169                 if (isp_dev->stream_count) {
1170                         sel->r = *rect;
1171                         mutex_unlock(&isp_dev->stream_lock);
1172                         ret = 0;
1173                         goto out;
1174                 } else {
1175                         isp_try_itiws(isp_dev, state, &sel->r, sel->which);
1176                         *rect = sel->r;
1177                 }
1178                 mutex_unlock(&isp_dev->stream_lock);
1179
1180                 /* Reset source pad format width and height */
1181                 fmt.which = sel->which;
1182                 fmt.pad = sel->pad;
1183                 ret = isp_get_format(sd, state, &fmt);
1184                 if (ret < 0)
1185                         return ret;
1186
1187                 fmt.format.width = rect->width;
1188                 fmt.format.height = rect->height;
1189                 ret = isp_set_format(sd, state, &fmt);
1190         } else if (sel->target == V4L2_SEL_TGT_CROP) {
1191                 struct v4l2_subdev_selection compose = { 0 };
1192                 int i;
1193
1194                 rect = __isp_get_crop(isp_dev, state, sel->which);
1195                 if (rect == NULL)
1196                         return -EINVAL;
1197
1198                 mutex_lock(&isp_dev->stream_lock);
1199                 if (isp_dev->stream_count) {
1200                         sel->r = *rect;
1201                         mutex_unlock(&isp_dev->stream_lock);
1202                         ret = 0;
1203                         goto out;
1204                 } else {
1205                         isp_try_crop(isp_dev, state, &sel->r, sel->which);
1206                         *rect = sel->r;
1207                 }
1208                 mutex_unlock(&isp_dev->stream_lock);
1209
1210                 /* Reset source compose selection */
1211                 compose.which = sel->which;
1212                 compose.target = V4L2_SEL_TGT_COMPOSE;
1213                 compose.r.width = rect->width;
1214                 compose.r.height = rect->height;
1215                 compose.pad = STF_ISP_PAD_SINK;
1216                 ret = isp_set_selection(sd, state, &compose);
1217
1218                 /* Reset source pad format width and height */
1219                 for (i = STF_ISP_PAD_SRC_RAW; i < STF_ISP_PAD_MAX; i++) {
1220                         struct v4l2_subdev_format fmt = { 0 };
1221
1222                         fmt.which = sel->which;
1223                         fmt.pad = i;
1224                         ret = isp_get_format(sd, state, &fmt);
1225                         if (ret < 0)
1226                                 return ret;
1227
1228                         fmt.format.width = rect->width;
1229                         fmt.format.height = rect->height;
1230                         ret = isp_set_format(sd, state, &fmt);
1231                 }
1232         } else {
1233                 ret = -EINVAL;
1234         }
1235
1236         st_info(ST_ISP, "%s pad = %d, left = %d, %d, %d, %d\n",
1237                         __func__, sel->pad, sel->r.left, sel->r.top, sel->r.width, sel->r.height);
1238 out:
1239         return ret;
1240 }
1241
1242 static int isp_init_formats(struct v4l2_subdev *sd,
1243                         struct v4l2_subdev_fh *fh)
1244 {
1245         struct v4l2_subdev_format format = {
1246                 .pad = STF_ISP_PAD_SINK,
1247                 .which = fh ? V4L2_SUBDEV_FORMAT_TRY :
1248                                 V4L2_SUBDEV_FORMAT_ACTIVE,
1249                 .format = {
1250                         .code = MEDIA_BUS_FMT_RGB565_2X8_LE,
1251                         .width = 1920,
1252                         .height = 1080
1253                 }
1254         };
1255
1256         return isp_set_format(sd, fh ? fh->state : NULL, &format);
1257 }
1258
1259 static int isp_link_setup(struct media_entity *entity,
1260                         const struct media_pad *local,
1261                         const struct media_pad *remote, u32 flags)
1262 {
1263         if (flags & MEDIA_LNK_FL_ENABLED)
1264                 if (media_entity_remote_pad(local))
1265                         return -EBUSY;
1266         return 0;
1267 }
1268
1269 static int stf_isp_load_setfile(struct stf_isp_dev *isp_dev, char *file_name)
1270 {
1271         struct device *dev = isp_dev->stfcamss->dev;
1272         const struct firmware *fw;
1273         u8 *buf = NULL;
1274         int *regval_num;
1275         int ret;
1276
1277         st_debug(ST_ISP, "%s, file_name %s\n", __func__, file_name);
1278         ret = request_firmware(&fw, file_name, dev);
1279         if (ret < 0) {
1280                 st_err(ST_ISP, "firmware request failed (%d)\n", ret);
1281                 return ret;
1282         }
1283         buf = devm_kzalloc(dev, fw->size, GFP_KERNEL);
1284         if (!buf)
1285                 return -ENOMEM;
1286         memcpy(buf, fw->data, fw->size);
1287
1288         mutex_lock(&isp_dev->setfile_lock);
1289         if (isp_dev->setfile.state == 1)
1290                 devm_kfree(dev, isp_dev->setfile.data);
1291         isp_dev->setfile.data = buf;
1292         isp_dev->setfile.size = fw->size;
1293         isp_dev->setfile.state = 1;
1294         regval_num = (int *)&buf[fw->size - sizeof(unsigned int)];
1295         isp_dev->setfile.settings.regval_num = *regval_num;
1296         isp_dev->setfile.settings.regval = (struct regval_t *)buf;
1297         mutex_unlock(&isp_dev->setfile_lock);
1298
1299         st_debug(ST_ISP, "stf_isp setfile loaded size: %zu B, reg_nul: %d\n",
1300                         fw->size, isp_dev->setfile.settings.regval_num);
1301
1302         release_firmware(fw);
1303         return ret;
1304 }
1305
1306 static long stf_isp_ioctl(struct v4l2_subdev *sd, unsigned int cmd, void *arg)
1307 {
1308         struct stf_isp_dev *isp_dev = v4l2_get_subdevdata(sd);
1309         struct device *dev = isp_dev->stfcamss->dev;
1310         int ret = -ENOIOCTLCMD;
1311
1312         switch (cmd) {
1313         case VIDIOC_STFISP_LOAD_FW: {
1314                 struct stfisp_fw_info *fw_info = arg;
1315
1316                 if (IS_ERR(fw_info)) {
1317                         st_err(ST_ISP, "fw_info failed, params invaild\n");
1318                         return -EINVAL;
1319                 }
1320
1321                 ret = stf_isp_load_setfile(isp_dev, fw_info->filename);
1322                 break;
1323         }
1324         case VIDIOC_STF_DMABUF_ALLOC:
1325         case VIDIOC_STF_DMABUF_FREE:
1326                 ret = stf_dmabuf_ioctl(dev, cmd, arg);
1327                 break;
1328         case VIDIOC_STFISP_GET_REG:
1329                 ret = isp_dev->hw_ops->isp_reg_read(isp_dev, arg);
1330                 break;
1331         case VIDIOC_STFISP_SET_REG:
1332                 ret = isp_dev->hw_ops->isp_reg_write(isp_dev, arg);
1333                 break;
1334         case VIDIOC_STFISP_SHADOW_LOCK:
1335                 if (atomic_add_unless(&isp_dev->shadow_count, 1, 1))
1336                         ret = 0;
1337                 else
1338                         ret = -EBUSY;
1339                 st_debug(ST_ISP, "%s, %d, ret = %d\n", __func__, __LINE__, ret);
1340                 break;
1341         case VIDIOC_STFISP_SHADOW_UNLOCK:
1342                 if (atomic_dec_if_positive(&isp_dev->shadow_count) < 0)
1343                         ret = -EINVAL;
1344                 else
1345                         ret = 0;
1346                 st_debug(ST_ISP, "%s, %d, ret = %d\n", __func__, __LINE__, ret);
1347                 break;
1348         case VIDIOC_STFISP_SHADOW_UNLOCK_N_TRIGGER:
1349                 {
1350                         isp_dev->hw_ops->isp_shadow_trigger(isp_dev);
1351                         if (atomic_dec_if_positive(&isp_dev->shadow_count) < 0)
1352                                 ret = -EINVAL;
1353                         else
1354                                 ret = 0;
1355                         st_debug(ST_ISP, "%s, %d, ret = %d\n", __func__, __LINE__, ret);
1356                 }
1357                 break;
1358         case VIDIOC_STFISP_SET_USER_CONFIG_ISP:
1359                 st_debug(ST_ISP, "%s, %d set user_config_isp\n", __func__, __LINE__);
1360                 user_config_isp = 1;
1361                 break;
1362         default:
1363                 break;
1364         }
1365         return ret;
1366 }
1367
1368 int isp_close(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
1369 {
1370         struct stf_isp_dev *isp_dev = v4l2_get_subdevdata(sd);
1371
1372         st_debug(ST_ISP, "%s, %d\n", __func__, __LINE__);
1373         while (atomic_dec_if_positive(&isp_dev->shadow_count) > 0)
1374                 st_warn(ST_ISP, "user not unlocked the shadow lock, driver unlock it!\n");
1375
1376         return 0;
1377 }
1378
1379 static int stf_isp_subscribe_event(struct v4l2_subdev *sd,
1380                                    struct v4l2_fh *fh,
1381                                    struct v4l2_event_subscription *sub)
1382 {
1383         switch (sub->type) {
1384         case V4L2_EVENT_SOURCE_CHANGE:
1385                 return v4l2_src_change_event_subdev_subscribe(sd, fh, sub);
1386         case V4L2_EVENT_CTRL:
1387                 return v4l2_ctrl_subdev_subscribe_event(sd, fh, sub);
1388         default:
1389                 st_debug(ST_ISP, "unspport subscribe_event\n");
1390                 return -EINVAL;
1391         }
1392 }
1393
1394 static const struct v4l2_subdev_core_ops isp_core_ops = {
1395         .s_power = isp_set_power,
1396         .ioctl = stf_isp_ioctl,
1397         .log_status = v4l2_ctrl_subdev_log_status,
1398         // .subscribe_event = v4l2_ctrl_subdev_subscribe_event,
1399         .subscribe_event = stf_isp_subscribe_event,
1400         .unsubscribe_event = v4l2_event_subdev_unsubscribe,
1401 };
1402
1403 static const struct v4l2_subdev_video_ops isp_video_ops = {
1404         .s_stream = isp_set_stream,
1405 };
1406
1407 static const struct v4l2_subdev_pad_ops isp_pad_ops = {
1408         .enum_mbus_code = isp_enum_mbus_code,
1409         .enum_frame_size = isp_enum_frame_size,
1410         .get_fmt = isp_get_format,
1411         .set_fmt = isp_set_format,
1412         .get_selection = isp_get_selection,
1413         .set_selection = isp_set_selection,
1414 };
1415
1416 static const struct v4l2_subdev_ops isp_v4l2_ops = {
1417         .core = &isp_core_ops,
1418         .video = &isp_video_ops,
1419         .pad = &isp_pad_ops,
1420 };
1421
1422 static const struct v4l2_subdev_internal_ops isp_v4l2_internal_ops = {
1423         .open = isp_init_formats,
1424         .close = isp_close,
1425 };
1426
1427 static const struct media_entity_operations isp_media_ops = {
1428         .link_setup = isp_link_setup,
1429         .link_validate = v4l2_subdev_link_validate,
1430 };
1431
1432 int stf_isp_register(struct stf_isp_dev *isp_dev,
1433                 struct v4l2_device *v4l2_dev)
1434 {
1435         struct v4l2_subdev *sd = &isp_dev->subdev;
1436         struct media_pad *pads = isp_dev->pads;
1437         int ret;
1438
1439         v4l2_subdev_init(sd, &isp_v4l2_ops);
1440         sd->internal_ops = &isp_v4l2_internal_ops;
1441         sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE | V4L2_SUBDEV_FL_HAS_EVENTS;
1442         snprintf(sd->name, ARRAY_SIZE(sd->name), "%s%d",
1443                 STF_ISP_NAME, 0);
1444         v4l2_set_subdevdata(sd, isp_dev);
1445
1446         ret = isp_init_formats(sd, NULL);
1447         if (ret < 0) {
1448                 st_err(ST_ISP, "Failed to init format: %d\n", ret);
1449                 return ret;
1450         }
1451
1452         pads[STF_ISP_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
1453         pads[STF_ISP_PAD_SRC].flags = MEDIA_PAD_FL_SOURCE;
1454         pads[STF_ISP_PAD_SRC_SS0].flags = MEDIA_PAD_FL_SOURCE;
1455         pads[STF_ISP_PAD_SRC_SS1].flags = MEDIA_PAD_FL_SOURCE;
1456         pads[STF_ISP_PAD_SRC_ITIW].flags = MEDIA_PAD_FL_SOURCE;
1457         pads[STF_ISP_PAD_SRC_ITIR].flags = MEDIA_PAD_FL_SOURCE;
1458         pads[STF_ISP_PAD_SRC_RAW].flags = MEDIA_PAD_FL_SOURCE;
1459         pads[STF_ISP_PAD_SRC_SCD_Y].flags = MEDIA_PAD_FL_SOURCE;
1460
1461         sd->entity.function = MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER;
1462         sd->entity.ops = &isp_media_ops;
1463         ret = media_entity_pads_init(&sd->entity, STF_ISP_PAD_MAX, pads);
1464         if (ret < 0) {
1465                 st_err(ST_ISP, "Failed to init media entity: %d\n", ret);
1466                 return ret;
1467         }
1468
1469         ret = isp_init_controls(isp_dev);
1470         if (ret)
1471                 goto err_sreg;
1472
1473         ret = v4l2_device_register_subdev(v4l2_dev, sd);
1474         if (ret < 0) {
1475                 st_err(ST_ISP, "Failed to register subdev: %d\n", ret);
1476                 goto free_ctrls;
1477         }
1478
1479         return 0;
1480
1481 free_ctrls:
1482         v4l2_ctrl_handler_free(&isp_dev->ctrls.handler);
1483 err_sreg:
1484         media_entity_cleanup(&sd->entity);
1485         return ret;
1486 }
1487
1488 int stf_isp_unregister(struct stf_isp_dev *isp_dev)
1489 {
1490         v4l2_device_unregister_subdev(&isp_dev->subdev);
1491         media_entity_cleanup(&isp_dev->subdev.entity);
1492         v4l2_ctrl_handler_free(&isp_dev->ctrls.handler);
1493         mutex_destroy(&isp_dev->stream_lock);
1494         mutex_destroy(&isp_dev->power_lock);
1495         mutex_destroy(&isp_dev->setfile_lock);
1496         return 0;
1497 }