drivers/media/platform/qcom/camss/camss-vfe-gen1.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /*
   3  * camss-vfe-gen1.c
   4  *
   5  * Qualcomm MSM Camera Subsystem - VFE Common functionality for Gen 1 versions of hw (4.1, 4.7..)
   6  *
   7  * Copyright (C) 2020 Linaro Ltd.
   8  */
   9
  10 #include "camss.h"
  11 #include "camss-vfe.h"
  12 #include "camss-vfe-gen1.h"
  13
  14 /* Max number of frame drop updates per frame */
  15 #define VFE_FRAME_DROP_UPDATES 2
  16 #define VFE_NEXT_SOF_MS 500
  17
  18 int vfe_gen1_halt(struct vfe_device *vfe)
  19 {
  20         unsigned long time;
  21
  22         reinit_completion(&vfe->halt_complete);
  23
  24         vfe->ops_gen1->halt_request(vfe);
  25
  26         time = wait_for_completion_timeout(&vfe->halt_complete,
  27                                            msecs_to_jiffies(VFE_HALT_TIMEOUT_MS));
  28         if (!time) {
  29                 dev_err(vfe->camss->dev, "VFE halt timeout\n");
  30                 return -EIO;
  31         }
  32
  33         return 0;
  34 }
  35
  36 static int vfe_disable_output(struct vfe_line *line)
  37 {
  38         struct vfe_device *vfe = to_vfe(line);
  39         struct vfe_output *output = &line->output;
  40         const struct vfe_hw_ops *ops = vfe->ops;
  41         unsigned long flags;
  42         unsigned long time;
  43         unsigned int i;
  44
  45         spin_lock_irqsave(&vfe->output_lock, flags);
  46
  47         output->gen1.wait_sof = 1;
  48         spin_unlock_irqrestore(&vfe->output_lock, flags);
  49
  50         time = wait_for_completion_timeout(&output->sof, msecs_to_jiffies(VFE_NEXT_SOF_MS));
  51         if (!time)
  52                 dev_err(vfe->camss->dev, "VFE sof timeout\n");
  53
  54         spin_lock_irqsave(&vfe->output_lock, flags);
  55         for (i = 0; i < output->wm_num; i++)
  56                 vfe->ops_gen1->wm_enable(vfe, output->wm_idx[i], 0);
  57
  58         ops->reg_update(vfe, line->id);
  59         output->wait_reg_update = 1;
  60         spin_unlock_irqrestore(&vfe->output_lock, flags);
  61
  62         time = wait_for_completion_timeout(&output->reg_update, msecs_to_jiffies(VFE_NEXT_SOF_MS));
  63         if (!time)
  64                 dev_err(vfe->camss->dev, "VFE reg update timeout\n");
  65
  66         spin_lock_irqsave(&vfe->output_lock, flags);
  67
  68         if (line->id != VFE_LINE_PIX) {
  69                 vfe->ops_gen1->wm_frame_based(vfe, output->wm_idx[0], 0);
  70                 vfe->ops_gen1->bus_disconnect_wm_from_rdi(vfe, output->wm_idx[0], line->id);
  71                 vfe->ops_gen1->enable_irq_wm_line(vfe, output->wm_idx[0], line->id, 0);
  72                 vfe->ops_gen1->set_cgc_override(vfe, output->wm_idx[0], 0);
  73                 spin_unlock_irqrestore(&vfe->output_lock, flags);
  74         } else {
  75                 for (i = 0; i < output->wm_num; i++) {
  76                         vfe->ops_gen1->wm_line_based(vfe, output->wm_idx[i], NULL, i, 0);
  77                         vfe->ops_gen1->set_cgc_override(vfe, output->wm_idx[i], 0);
  78                 }
  79
  80                 vfe->ops_gen1->enable_irq_pix_line(vfe, 0, line->id, 0);
  81                 vfe->ops_gen1->set_module_cfg(vfe, 0);
  82                 vfe->ops_gen1->set_realign_cfg(vfe, line, 0);
  83                 vfe->ops_gen1->set_xbar_cfg(vfe, output, 0);
  84                 vfe->ops_gen1->set_camif_cmd(vfe, 0);
  85
  86                 spin_unlock_irqrestore(&vfe->output_lock, flags);
  87
  88                 vfe->ops_gen1->camif_wait_for_stop(vfe, vfe->camss->dev);
  89         }
  90
  91         return 0;
  92 }
  93
  94 /*
  95  * vfe_gen1_disable - Disable streaming on VFE line
  96  * @line: VFE line
  97  *
  98  * Return 0 on success or a negative error code otherwise
  99  */
 100 int vfe_gen1_disable(struct vfe_line *line)
 101 {
 102         struct vfe_device *vfe = to_vfe(line);
 103
 104         vfe_disable_output(line);
 105
 106         vfe_put_output(line);
 107
 108         mutex_lock(&vfe->stream_lock);
 109
 110         if (vfe->stream_count == 1)
 111                 vfe->ops_gen1->bus_enable_wr_if(vfe, 0);
 112
 113         vfe->stream_count--;
 114
 115         mutex_unlock(&vfe->stream_lock);
 116
 117         return 0;
 118 }
 119
 120 static void vfe_output_init_addrs(struct vfe_device *vfe,
 121                                   struct vfe_output *output, u8 sync,
 122                                   struct vfe_line *line)
 123 {
 124         u32 ping_addr;
 125         u32 pong_addr;
 126         unsigned int i;
 127
 128         output->gen1.active_buf = 0;
 129
 130         for (i = 0; i < output->wm_num; i++) {
 131                 if (output->buf[0])
 132                         ping_addr = output->buf[0]->addr[i];
 133                 else
 134                         ping_addr = 0;
 135
 136                 if (output->buf[1])
 137                         pong_addr = output->buf[1]->addr[i];
 138                 else
 139                         pong_addr = ping_addr;
 140
 141                 vfe->ops_gen1->wm_set_ping_addr(vfe, output->wm_idx[i], ping_addr);
 142                 vfe->ops_gen1->wm_set_pong_addr(vfe, output->wm_idx[i], pong_addr);
 143                 if (sync)
 144                         vfe->ops_gen1->bus_reload_wm(vfe, output->wm_idx[i]);
 145         }
 146 }
 147
 148 static void vfe_output_frame_drop(struct vfe_device *vfe,
 149                                   struct vfe_output *output,
 150                                   u32 drop_pattern)
 151 {
 152         u8 drop_period;
 153         unsigned int i;
 154
 155         /* We need to toggle update period to be valid on next frame */
 156         output->drop_update_idx++;
 157         output->drop_update_idx %= VFE_FRAME_DROP_UPDATES;
 158         drop_period = VFE_FRAME_DROP_VAL + output->drop_update_idx;
 159
 160         for (i = 0; i < output->wm_num; i++) {
 161                 vfe->ops_gen1->wm_set_framedrop_period(vfe, output->wm_idx[i], drop_period);
 162                 vfe->ops_gen1->wm_set_framedrop_pattern(vfe, output->wm_idx[i], drop_pattern);
 163         }
 164
 165         vfe->ops->reg_update(vfe, container_of(output, struct vfe_line, output)->id);
 166 }
 167
 168 static int vfe_enable_output(struct vfe_line *line)
 169 {
 170         struct vfe_device *vfe = to_vfe(line);
 171         struct vfe_output *output = &line->output;
 172         const struct vfe_hw_ops *ops = vfe->ops;
 173         struct media_entity *sensor;
 174         unsigned long flags;
 175         unsigned int frame_skip = 0;
 176         unsigned int i;
 177         u16 ub_size;
 178
 179         ub_size = vfe->ops_gen1->get_ub_size(vfe->id);
 180         if (!ub_size)
 181                 return -EINVAL;
 182
 183         sensor = camss_find_sensor(&line->subdev.entity);
 184         if (sensor) {
 185                 struct v4l2_subdev *subdev = media_entity_to_v4l2_subdev(sensor);
 186
 187                 v4l2_subdev_call(subdev, sensor, g_skip_frames, &frame_skip);
 188                 /* Max frame skip is 29 frames */
 189                 if (frame_skip > VFE_FRAME_DROP_VAL - 1)
 190                         frame_skip = VFE_FRAME_DROP_VAL - 1;
 191         }
 192
 193         spin_lock_irqsave(&vfe->output_lock, flags);
 194
 195         ops->reg_update_clear(vfe, line->id);
 196
 197         if (output->state > VFE_OUTPUT_RESERVED) {
 198                 dev_err(vfe->camss->dev, "Output is not in reserved state %d\n", output->state);
 199                 spin_unlock_irqrestore(&vfe->output_lock, flags);
 200                 return -EINVAL;
 201         }
 202         output->state = VFE_OUTPUT_IDLE;
 203
 204         output->buf[0] = vfe_buf_get_pending(output);
 205         output->buf[1] = vfe_buf_get_pending(output);
 206
 207         if (!output->buf[0] && output->buf[1]) {
 208                 output->buf[0] = output->buf[1];
 209                 output->buf[1] = NULL;
 210         }
 211
 212         if (output->buf[0])
 213                 output->state = VFE_OUTPUT_SINGLE;
 214
 215         if (output->buf[1])
 216                 output->state = VFE_OUTPUT_CONTINUOUS;
 217
 218         switch (output->state) {
 219         case VFE_OUTPUT_SINGLE:
 220                 vfe_output_frame_drop(vfe, output, 1 << frame_skip);
 221                 break;
 222         case VFE_OUTPUT_CONTINUOUS:
 223                 vfe_output_frame_drop(vfe, output, 3 << frame_skip);
 224                 break;
 225         default:
 226                 vfe_output_frame_drop(vfe, output, 0);
 227                 break;
 228         }
 229
 230         output->sequence = 0;
 231         output->gen1.wait_sof = 0;
 232         output->wait_reg_update = 0;
 233         reinit_completion(&output->sof);
 234         reinit_completion(&output->reg_update);
 235
 236         vfe_output_init_addrs(vfe, output, 0, line);
 237
 238         if (line->id != VFE_LINE_PIX) {
 239                 vfe->ops_gen1->set_cgc_override(vfe, output->wm_idx[0], 1);
 240                 vfe->ops_gen1->enable_irq_wm_line(vfe, output->wm_idx[0], line->id, 1);
 241                 vfe->ops_gen1->bus_connect_wm_to_rdi(vfe, output->wm_idx[0], line->id);
 242                 vfe->ops_gen1->wm_set_subsample(vfe, output->wm_idx[0]);
 243                 vfe->ops_gen1->set_rdi_cid(vfe, line->id, 0);
 244                 vfe->ops_gen1->wm_set_ub_cfg(vfe, output->wm_idx[0],
 245                                             (ub_size + 1) * output->wm_idx[0], ub_size);
 246                 vfe->ops_gen1->wm_frame_based(vfe, output->wm_idx[0], 1);
 247                 vfe->ops_gen1->wm_enable(vfe, output->wm_idx[0], 1);
 248                 vfe->ops_gen1->bus_reload_wm(vfe, output->wm_idx[0]);
 249         } else {
 250                 ub_size /= output->wm_num;
 251                 for (i = 0; i < output->wm_num; i++) {
 252                         vfe->ops_gen1->set_cgc_override(vfe, output->wm_idx[i], 1);
 253                         vfe->ops_gen1->wm_set_subsample(vfe, output->wm_idx[i]);
 254                         vfe->ops_gen1->wm_set_ub_cfg(vfe, output->wm_idx[i],
 255                                                      (ub_size + 1) * output->wm_idx[i], ub_size);
 256                         vfe->ops_gen1->wm_line_based(vfe, output->wm_idx[i],
 257                                                      &line->video_out.active_fmt.fmt.pix_mp, i, 1);
 258                         vfe->ops_gen1->wm_enable(vfe, output->wm_idx[i], 1);
 259                         vfe->ops_gen1->bus_reload_wm(vfe, output->wm_idx[i]);
 260                 }
 261                 vfe->ops_gen1->enable_irq_pix_line(vfe, 0, line->id, 1);
 262                 vfe->ops_gen1->set_module_cfg(vfe, 1);
 263                 vfe->ops_gen1->set_camif_cfg(vfe, line);
 264                 vfe->ops_gen1->set_realign_cfg(vfe, line, 1);
 265                 vfe->ops_gen1->set_xbar_cfg(vfe, output, 1);
 266                 vfe->ops_gen1->set_demux_cfg(vfe, line);
 267                 vfe->ops_gen1->set_scale_cfg(vfe, line);
 268                 vfe->ops_gen1->set_crop_cfg(vfe, line);
 269                 vfe->ops_gen1->set_clamp_cfg(vfe);
 270                 vfe->ops_gen1->set_camif_cmd(vfe, 1);
 271         }
 272
 273         ops->reg_update(vfe, line->id);
 274
 275         spin_unlock_irqrestore(&vfe->output_lock, flags);
 276
 277         return 0;
 278 }
 279
 280 static int vfe_get_output(struct vfe_line *line)
 281 {
 282         struct vfe_device *vfe = to_vfe(line);
 283         struct vfe_output *output;
 284         struct v4l2_format *f = &line->video_out.active_fmt;
 285         unsigned long flags;
 286         int i;
 287         int wm_idx;
 288
 289         spin_lock_irqsave(&vfe->output_lock, flags);
 290
 291         output = &line->output;
 292         if (output->state > VFE_OUTPUT_RESERVED) {
 293                 dev_err(vfe->camss->dev, "Output is running\n");
 294                 goto error;
 295         }
 296         output->state = VFE_OUTPUT_RESERVED;
 297
 298         output->gen1.active_buf = 0;
 299
 300         switch (f->fmt.pix_mp.pixelformat) {
 301         case V4L2_PIX_FMT_NV12:
 302         case V4L2_PIX_FMT_NV21:
 303         case V4L2_PIX_FMT_NV16:
 304         case V4L2_PIX_FMT_NV61:
 305                 output->wm_num = 2;
 306                 break;
 307         default:
 308                 output->wm_num = 1;
 309                 break;
 310         }
 311
 312         for (i = 0; i < output->wm_num; i++) {
 313                 wm_idx = vfe_reserve_wm(vfe, line->id);
 314                 if (wm_idx < 0) {
 315                         dev_err(vfe->camss->dev, "Can not reserve wm\n");
 316                         goto error_get_wm;
 317                 }
 318                 output->wm_idx[i] = wm_idx;
 319         }
 320
 321         output->drop_update_idx = 0;
 322
 323         spin_unlock_irqrestore(&vfe->output_lock, flags);
 324
 325         return 0;
 326
 327 error_get_wm:
 328         for (i--; i >= 0; i--)
 329                 vfe_release_wm(vfe, output->wm_idx[i]);
 330         output->state = VFE_OUTPUT_OFF;
 331 error:
 332         spin_unlock_irqrestore(&vfe->output_lock, flags);
 333
 334         return -EINVAL;
 335 }
 336
 337 int vfe_gen1_enable(struct vfe_line *line)
 338 {
 339         struct vfe_device *vfe = to_vfe(line);
 340         int ret;
 341
 342         mutex_lock(&vfe->stream_lock);
 343
 344         if (!vfe->stream_count) {
 345                 vfe->ops_gen1->enable_irq_common(vfe);
 346                 vfe->ops_gen1->bus_enable_wr_if(vfe, 1);
 347                 vfe->ops_gen1->set_qos(vfe);
 348                 vfe->ops_gen1->set_ds(vfe);
 349         }
 350
 351         vfe->stream_count++;
 352
 353         mutex_unlock(&vfe->stream_lock);
 354
 355         ret = vfe_get_output(line);
 356         if (ret < 0)
 357                 goto error_get_output;
 358
 359         ret = vfe_enable_output(line);
 360         if (ret < 0)
 361                 goto error_enable_output;
 362
 363         vfe->was_streaming = 1;
 364
 365         return 0;
 366
 367 error_enable_output:
 368         vfe_put_output(line);
 369
 370 error_get_output:
 371         mutex_lock(&vfe->stream_lock);
 372
 373         if (vfe->stream_count == 1)
 374                 vfe->ops_gen1->bus_enable_wr_if(vfe, 0);
 375
 376         vfe->stream_count--;
 377
 378         mutex_unlock(&vfe->stream_lock);
 379
 380         return ret;
 381 }
 382
 383 static void vfe_output_update_ping_addr(struct vfe_device *vfe,
 384                                         struct vfe_output *output, u8 sync,
 385                                         struct vfe_line *line)
 386 {
 387         u32 addr;
 388         unsigned int i;
 389
 390         for (i = 0; i < output->wm_num; i++) {
 391                 if (output->buf[0])
 392                         addr = output->buf[0]->addr[i];
 393                 else
 394                         addr = 0;
 395
 396                 vfe->ops_gen1->wm_set_ping_addr(vfe, output->wm_idx[i], addr);
 397                 if (sync)
 398                         vfe->ops_gen1->bus_reload_wm(vfe, output->wm_idx[i]);
 399         }
 400 }
 401
 402 static void vfe_output_update_pong_addr(struct vfe_device *vfe,
 403                                         struct vfe_output *output, u8 sync,
 404                                         struct vfe_line *line)
 405 {
 406         u32 addr;
 407         unsigned int i;
 408
 409         for (i = 0; i < output->wm_num; i++) {
 410                 if (output->buf[1])
 411                         addr = output->buf[1]->addr[i];
 412                 else
 413                         addr = 0;
 414
 415                 vfe->ops_gen1->wm_set_pong_addr(vfe, output->wm_idx[i], addr);
 416                 if (sync)
 417                         vfe->ops_gen1->bus_reload_wm(vfe, output->wm_idx[i]);
 418         }
 419 }
 420
 421 static void vfe_buf_update_wm_on_next(struct vfe_device *vfe,
 422                                       struct vfe_output *output)
 423 {
 424         switch (output->state) {
 425         case VFE_OUTPUT_CONTINUOUS:
 426                 vfe_output_frame_drop(vfe, output, 3);
 427                 break;
 428         case VFE_OUTPUT_SINGLE:
 429         default:
 430                 dev_err_ratelimited(vfe->camss->dev,
 431                                     "Next buf in wrong state! %d\n",
 432                                     output->state);
 433                 break;
 434         }
 435 }
 436
 437 static void vfe_buf_update_wm_on_last(struct vfe_device *vfe,
 438                                       struct vfe_output *output)
 439 {
 440         switch (output->state) {
 441         case VFE_OUTPUT_CONTINUOUS:
 442                 output->state = VFE_OUTPUT_SINGLE;
 443                 vfe_output_frame_drop(vfe, output, 1);
 444                 break;
 445         case VFE_OUTPUT_SINGLE:
 446                 output->state = VFE_OUTPUT_STOPPING;
 447                 vfe_output_frame_drop(vfe, output, 0);
 448                 break;
 449         default:
 450                 dev_err_ratelimited(vfe->camss->dev,
 451                                     "Last buff in wrong state! %d\n",
 452                                     output->state);
 453                 break;
 454         }
 455 }
 456
 457 static void vfe_buf_update_wm_on_new(struct vfe_device *vfe,
 458                                      struct vfe_output *output,
 459                                      struct camss_buffer *new_buf,
 460                                      struct vfe_line *line)
 461 {
 462         int inactive_idx;
 463
 464         switch (output->state) {
 465         case VFE_OUTPUT_SINGLE:
 466                 inactive_idx = !output->gen1.active_buf;
 467
 468                 if (!output->buf[inactive_idx]) {
 469                         output->buf[inactive_idx] = new_buf;
 470
 471                         if (inactive_idx)
 472                                 vfe_output_update_pong_addr(vfe, output, 0, line);
 473                         else
 474                                 vfe_output_update_ping_addr(vfe, output, 0, line);
 475
 476                         vfe_output_frame_drop(vfe, output, 3);
 477                         output->state = VFE_OUTPUT_CONTINUOUS;
 478                 } else {
 479                         vfe_buf_add_pending(output, new_buf);
 480                         dev_err_ratelimited(vfe->camss->dev,
 481                                             "Inactive buffer is busy\n");
 482                 }
 483                 break;
 484
 485         case VFE_OUTPUT_IDLE:
 486                 if (!output->buf[0]) {
 487                         output->buf[0] = new_buf;
 488
 489                         vfe_output_init_addrs(vfe, output, 1, line);
 490                         vfe_output_frame_drop(vfe, output, 1);
 491
 492                         output->state = VFE_OUTPUT_SINGLE;
 493                 } else {
 494                         vfe_buf_add_pending(output, new_buf);
 495                         dev_err_ratelimited(vfe->camss->dev,
 496                                             "Output idle with buffer set!\n");
 497                 }
 498                 break;
 499
 500         case VFE_OUTPUT_CONTINUOUS:
 501         default:
 502                 vfe_buf_add_pending(output, new_buf);
 503                 break;
 504         }
 505 }
 506
 507 /*
 508  * vfe_isr_halt_ack - Process halt ack
 509  * @vfe: VFE Device
 510  */
 511 static void vfe_isr_halt_ack(struct vfe_device *vfe)
 512 {
 513         complete(&vfe->halt_complete);
 514         vfe->ops_gen1->halt_clear(vfe);
 515 }
 516
 517 /*
 518  * vfe_isr_sof - Process start of frame interrupt
 519  * @vfe: VFE Device
 520  * @line_id: VFE line
 521  */
 522 static void vfe_isr_sof(struct vfe_device *vfe, enum vfe_line_id line_id)
 523 {
 524         struct vfe_output *output;
 525         unsigned long flags;
 526
 527         spin_lock_irqsave(&vfe->output_lock, flags);
 528         output = &vfe->line[line_id].output;
 529         if (output->gen1.wait_sof) {
 530                 output->gen1.wait_sof = 0;
 531                 complete(&output->sof);
 532         }
 533         spin_unlock_irqrestore(&vfe->output_lock, flags);
 534 }
 535
 536 /*
 537  * vfe_isr_reg_update - Process reg update interrupt
 538  * @vfe: VFE Device
 539  * @line_id: VFE line
 540  */
 541 static void vfe_isr_reg_update(struct vfe_device *vfe, enum vfe_line_id line_id)
 542 {
 543         struct vfe_output *output;
 544         struct vfe_line *line = &vfe->line[line_id];
 545         unsigned long flags;
 546
 547         spin_lock_irqsave(&vfe->output_lock, flags);
 548         vfe->ops->reg_update_clear(vfe, line_id);
 549
 550         output = &line->output;
 551
 552         if (output->wait_reg_update) {
 553                 output->wait_reg_update = 0;
 554                 complete(&output->reg_update);
 555                 spin_unlock_irqrestore(&vfe->output_lock, flags);
 556                 return;
 557         }
 558
 559         if (output->state == VFE_OUTPUT_STOPPING) {
 560                 /* Release last buffer when hw is idle */
 561                 if (output->last_buffer) {
 562                         vb2_buffer_done(&output->last_buffer->vb.vb2_buf,
 563                                         VB2_BUF_STATE_DONE);
 564                         output->last_buffer = NULL;
 565                 }
 566                 output->state = VFE_OUTPUT_IDLE;
 567
 568                 /* Buffers received in stopping state are queued in */
 569                 /* dma pending queue, start next capture here */
 570
 571                 output->buf[0] = vfe_buf_get_pending(output);
 572                 output->buf[1] = vfe_buf_get_pending(output);
 573
 574                 if (!output->buf[0] && output->buf[1]) {
 575                         output->buf[0] = output->buf[1];
 576                         output->buf[1] = NULL;
 577                 }
 578
 579                 if (output->buf[0])
 580                         output->state = VFE_OUTPUT_SINGLE;
 581
 582                 if (output->buf[1])
 583                         output->state = VFE_OUTPUT_CONTINUOUS;
 584
 585                 switch (output->state) {
 586                 case VFE_OUTPUT_SINGLE:
 587                         vfe_output_frame_drop(vfe, output, 2);
 588                         break;
 589                 case VFE_OUTPUT_CONTINUOUS:
 590                         vfe_output_frame_drop(vfe, output, 3);
 591                         break;
 592                 default:
 593                         vfe_output_frame_drop(vfe, output, 0);
 594                         break;
 595                 }
 596
 597                 vfe_output_init_addrs(vfe, output, 1, &vfe->line[line_id]);
 598         }
 599
 600         spin_unlock_irqrestore(&vfe->output_lock, flags);
 601 }
 602
 603 /*
 604  * vfe_isr_wm_done - Process write master done interrupt
 605  * @vfe: VFE Device
 606  * @wm: Write master id
 607  */
 608 static void vfe_isr_wm_done(struct vfe_device *vfe, u8 wm)
 609 {
 610         struct camss_buffer *ready_buf;
 611         struct vfe_output *output;
 612         dma_addr_t *new_addr;
 613         unsigned long flags;
 614         u32 active_index;
 615         u64 ts = ktime_get_ns();
 616         unsigned int i;
 617
 618         active_index = vfe->ops_gen1->wm_get_ping_pong_status(vfe, wm);
 619
 620         spin_lock_irqsave(&vfe->output_lock, flags);
 621
 622         if (vfe->wm_output_map[wm] == VFE_LINE_NONE) {
 623                 dev_err_ratelimited(vfe->camss->dev,
 624                                     "Received wm done for unmapped index\n");
 625                 goto out_unlock;
 626         }
 627         output = &vfe->line[vfe->wm_output_map[wm]].output;
 628
 629         if (output->gen1.active_buf == active_index && 0) {
 630                 dev_err_ratelimited(vfe->camss->dev,
 631                                     "Active buffer mismatch!\n");
 632                 goto out_unlock;
 633         }
 634         output->gen1.active_buf = active_index;
 635
 636         ready_buf = output->buf[!active_index];
 637         if (!ready_buf) {
 638                 dev_err_ratelimited(vfe->camss->dev,
 639                                     "Missing ready buf %d %d!\n",
 640                                     !active_index, output->state);
 641                 goto out_unlock;
 642         }
 643
 644         ready_buf->vb.vb2_buf.timestamp = ts;
 645         ready_buf->vb.sequence = output->sequence++;
 646
 647         /* Get next buffer */
 648         output->buf[!active_index] = vfe_buf_get_pending(output);
 649         if (!output->buf[!active_index]) {
 650                 /* No next buffer - set same address */
 651                 new_addr = ready_buf->addr;
 652                 vfe_buf_update_wm_on_last(vfe, output);
 653         } else {
 654                 new_addr = output->buf[!active_index]->addr;
 655                 vfe_buf_update_wm_on_next(vfe, output);
 656         }
 657
 658         if (active_index)
 659                 for (i = 0; i < output->wm_num; i++)
 660                         vfe->ops_gen1->wm_set_ping_addr(vfe, output->wm_idx[i], new_addr[i]);
 661         else
 662                 for (i = 0; i < output->wm_num; i++)
 663                         vfe->ops_gen1->wm_set_pong_addr(vfe, output->wm_idx[i], new_addr[i]);
 664
 665         spin_unlock_irqrestore(&vfe->output_lock, flags);
 666
 667         if (output->state == VFE_OUTPUT_STOPPING)
 668                 output->last_buffer = ready_buf;
 669         else
 670                 vb2_buffer_done(&ready_buf->vb.vb2_buf, VB2_BUF_STATE_DONE);
 671
 672         return;
 673
 674 out_unlock:
 675         spin_unlock_irqrestore(&vfe->output_lock, flags);
 676 }
 677
 678 /*
 679  * vfe_queue_buffer - Add empty buffer
 680  * @vid: Video device structure
 681  * @buf: Buffer to be enqueued
 682  *
 683  * Add an empty buffer - depending on the current number of buffers it will be
 684  * put in pending buffer queue or directly given to the hardware to be filled.
 685  *
 686  * Return 0 on success or a negative error code otherwise
 687  */
 688 static int vfe_queue_buffer(struct camss_video *vid, struct camss_buffer *buf)
 689 {
 690         struct vfe_line *line = container_of(vid, struct vfe_line, video_out);
 691         struct vfe_device *vfe = to_vfe(line);
 692         struct vfe_output *output;
 693         unsigned long flags;
 694
 695         output = &line->output;
 696
 697         spin_lock_irqsave(&vfe->output_lock, flags);
 698
 699         vfe_buf_update_wm_on_new(vfe, output, buf, line);
 700
 701         spin_unlock_irqrestore(&vfe->output_lock, flags);
 702
 703         return 0;
 704 }
 705
 706 #define CALC_WORD(width, M, N) (((width) * (M) + (N) - 1) / (N))
 707
 708 int vfe_word_per_line(u32 format, u32 width)
 709 {
 710         int val = 0;
 711
 712         switch (format) {
 713         case V4L2_PIX_FMT_NV12:
 714         case V4L2_PIX_FMT_NV21:
 715         case V4L2_PIX_FMT_NV16:
 716         case V4L2_PIX_FMT_NV61:
 717                 val = CALC_WORD(width, 1, 8);
 718                 break;
 719         case V4L2_PIX_FMT_YUYV:
 720         case V4L2_PIX_FMT_YVYU:
 721         case V4L2_PIX_FMT_UYVY:
 722         case V4L2_PIX_FMT_VYUY:
 723                 val = CALC_WORD(width, 2, 8);
 724                 break;
 725         }
 726
 727         return val;
 728 }
 729
 730 const struct vfe_isr_ops vfe_isr_ops_gen1 = {
 731         .reset_ack = vfe_isr_reset_ack,
 732         .halt_ack = vfe_isr_halt_ack,
 733         .reg_update = vfe_isr_reg_update,
 734         .sof = vfe_isr_sof,
 735         .comp_done = vfe_isr_comp_done,
 736         .wm_done = vfe_isr_wm_done,
 737 };
 738
 739 const struct camss_video_ops vfe_video_ops_gen1 = {
 740         .queue_buffer = vfe_queue_buffer,
 741         .flush_buffers = vfe_flush_buffers,
 742 };