drivers/gpu/drm/kmb/kmb_dsi.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2 /*
   3  * Copyright © 2019-2020 Intel Corporation
   4  */
   5
   6 #include <linux/clk.h>
   7 #include <linux/delay.h>
   8 #include <linux/of_graph.h>
   9 #include <linux/mfd/syscon.h>
  10 #include <linux/platform_device.h>
  11 #include <linux/regmap.h>
  12
  13 #include <drm/drm_atomic_helper.h>
  14 #include <drm/drm_bridge.h>
  15 #include <drm/drm_bridge_connector.h>
  16 #include <drm/drm_mipi_dsi.h>
  17 #include <drm/drm_simple_kms_helper.h>
  18 #include <drm/drm_print.h>
  19 #include <drm/drm_probe_helper.h>
  20
  21 #include "kmb_dsi.h"
  22 #include "kmb_regs.h"
  23
  24 static struct mipi_dsi_host *dsi_host;
  25 static struct mipi_dsi_device *dsi_device;
  26 static struct drm_bridge *adv_bridge;
  27
  28 /* Default setting is 1080p, 4 lanes */
  29 #define IMG_HEIGHT_LINES  1080
  30 #define IMG_WIDTH_PX      1920
  31 #define MIPI_TX_ACTIVE_LANES 4
  32
  33 static struct mipi_tx_frame_section_cfg mipi_tx_frame0_sect_cfg = {
  34         .width_pixels = IMG_WIDTH_PX,
  35         .height_lines = IMG_HEIGHT_LINES,
  36         .data_type = DSI_LP_DT_PPS_RGB888_24B,
  37         .data_mode = MIPI_DATA_MODE1,
  38         .dma_packed = 0
  39 };
  40
  41 static struct mipi_tx_frame_cfg mipitx_frame0_cfg = {
  42         .sections[0] = &mipi_tx_frame0_sect_cfg,
  43         .sections[1] = NULL,
  44         .sections[2] = NULL,
  45         .sections[3] = NULL,
  46         .vsync_width = 5,
  47         .v_backporch = 36,
  48         .v_frontporch = 4,
  49         .hsync_width = 44,
  50         .h_backporch = 148,
  51         .h_frontporch = 88
  52 };
  53
  54 static const struct mipi_tx_dsi_cfg mipitx_dsi_cfg = {
  55         .hfp_blank_en = 0,
  56         .eotp_en = 0,
  57         .lpm_last_vfp_line = 0,
  58         .lpm_first_vsa_line = 0,
  59         .sync_pulse_eventn = DSI_VIDEO_MODE_NO_BURST_EVENT,
  60         .hfp_blanking = SEND_BLANK_PACKET,
  61         .hbp_blanking = SEND_BLANK_PACKET,
  62         .hsa_blanking = SEND_BLANK_PACKET,
  63         .v_blanking = SEND_BLANK_PACKET,
  64 };
  65
  66 static struct mipi_ctrl_cfg mipi_tx_init_cfg = {
  67         .active_lanes = MIPI_TX_ACTIVE_LANES,
  68         .lane_rate_mbps = MIPI_TX_LANE_DATA_RATE_MBPS,
  69         .ref_clk_khz = MIPI_TX_REF_CLK_KHZ,
  70         .cfg_clk_khz = MIPI_TX_CFG_CLK_KHZ,
  71         .tx_ctrl_cfg = {
  72                         .frames[0] = &mipitx_frame0_cfg,
  73                         .frames[1] = NULL,
  74                         .frames[2] = NULL,
  75                         .frames[3] = NULL,
  76                         .tx_dsi_cfg = &mipitx_dsi_cfg,
  77                         .line_sync_pkt_en = 0,
  78                         .line_counter_active = 0,
  79                         .frame_counter_active = 0,
  80                         .tx_always_use_hact = 1,
  81                         .tx_hact_wait_stop = 1,
  82                         }
  83 };
  84
  85 struct  mipi_hs_freq_range_cfg {
  86         u16 default_bit_rate_mbps;
  87         u8 hsfreqrange_code;
  88 };
  89
  90 struct vco_params {
  91         u32 freq;
  92         u32 range;
  93         u32 divider;
  94 };
  95
  96 static const struct vco_params vco_table[] = {
  97         {52, 0x3f, 8},
  98         {80, 0x39, 8},
  99         {105, 0x2f, 4},
 100         {160, 0x29, 4},
 101         {210, 0x1f, 2},
 102         {320, 0x19, 2},
 103         {420, 0x0f, 1},
 104         {630, 0x09, 1},
 105         {1100, 0x03, 1},
 106         {0xffff, 0x01, 1},
 107 };
 108
 109 static const struct mipi_hs_freq_range_cfg
 110 mipi_hs_freq_range[MIPI_DPHY_DEFAULT_BIT_RATES] = {
 111         {.default_bit_rate_mbps = 80, .hsfreqrange_code = 0x00},
 112         {.default_bit_rate_mbps = 90, .hsfreqrange_code = 0x10},
 113         {.default_bit_rate_mbps = 100, .hsfreqrange_code = 0x20},
 114         {.default_bit_rate_mbps = 110, .hsfreqrange_code = 0x30},
 115         {.default_bit_rate_mbps = 120, .hsfreqrange_code = 0x01},
 116         {.default_bit_rate_mbps = 130, .hsfreqrange_code = 0x11},
 117         {.default_bit_rate_mbps = 140, .hsfreqrange_code = 0x21},
 118         {.default_bit_rate_mbps = 150, .hsfreqrange_code = 0x31},
 119         {.default_bit_rate_mbps = 160, .hsfreqrange_code = 0x02},
 120         {.default_bit_rate_mbps = 170, .hsfreqrange_code = 0x12},
 121         {.default_bit_rate_mbps = 180, .hsfreqrange_code = 0x22},
 122         {.default_bit_rate_mbps = 190, .hsfreqrange_code = 0x32},
 123         {.default_bit_rate_mbps = 205, .hsfreqrange_code = 0x03},
 124         {.default_bit_rate_mbps = 220, .hsfreqrange_code = 0x13},
 125         {.default_bit_rate_mbps = 235, .hsfreqrange_code = 0x23},
 126         {.default_bit_rate_mbps = 250, .hsfreqrange_code = 0x33},
 127         {.default_bit_rate_mbps = 275, .hsfreqrange_code = 0x04},
 128         {.default_bit_rate_mbps = 300, .hsfreqrange_code = 0x14},
 129         {.default_bit_rate_mbps = 325, .hsfreqrange_code = 0x25},
 130         {.default_bit_rate_mbps = 350, .hsfreqrange_code = 0x35},
 131         {.default_bit_rate_mbps = 400, .hsfreqrange_code = 0x05},
 132         {.default_bit_rate_mbps = 450, .hsfreqrange_code = 0x16},
 133         {.default_bit_rate_mbps = 500, .hsfreqrange_code = 0x26},
 134         {.default_bit_rate_mbps = 550, .hsfreqrange_code = 0x37},
 135         {.default_bit_rate_mbps = 600, .hsfreqrange_code = 0x07},
 136         {.default_bit_rate_mbps = 650, .hsfreqrange_code = 0x18},
 137         {.default_bit_rate_mbps = 700, .hsfreqrange_code = 0x28},
 138         {.default_bit_rate_mbps = 750, .hsfreqrange_code = 0x39},
 139         {.default_bit_rate_mbps = 800, .hsfreqrange_code = 0x09},
 140         {.default_bit_rate_mbps = 850, .hsfreqrange_code = 0x19},
 141         {.default_bit_rate_mbps = 900, .hsfreqrange_code = 0x29},
 142         {.default_bit_rate_mbps = 1000, .hsfreqrange_code = 0x0A},
 143         {.default_bit_rate_mbps = 1050, .hsfreqrange_code = 0x1A},
 144         {.default_bit_rate_mbps = 1100, .hsfreqrange_code = 0x2A},
 145         {.default_bit_rate_mbps = 1150, .hsfreqrange_code = 0x3B},
 146         {.default_bit_rate_mbps = 1200, .hsfreqrange_code = 0x0B},
 147         {.default_bit_rate_mbps = 1250, .hsfreqrange_code = 0x1B},
 148         {.default_bit_rate_mbps = 1300, .hsfreqrange_code = 0x2B},
 149         {.default_bit_rate_mbps = 1350, .hsfreqrange_code = 0x3C},
 150         {.default_bit_rate_mbps = 1400, .hsfreqrange_code = 0x0C},
 151         {.default_bit_rate_mbps = 1450, .hsfreqrange_code = 0x1C},
 152         {.default_bit_rate_mbps = 1500, .hsfreqrange_code = 0x2C},
 153         {.default_bit_rate_mbps = 1550, .hsfreqrange_code = 0x3D},
 154         {.default_bit_rate_mbps = 1600, .hsfreqrange_code = 0x0D},
 155         {.default_bit_rate_mbps = 1650, .hsfreqrange_code = 0x1D},
 156         {.default_bit_rate_mbps = 1700, .hsfreqrange_code = 0x2E},
 157         {.default_bit_rate_mbps = 1750, .hsfreqrange_code = 0x3E},
 158         {.default_bit_rate_mbps = 1800, .hsfreqrange_code = 0x0E},
 159         {.default_bit_rate_mbps = 1850, .hsfreqrange_code = 0x1E},
 160         {.default_bit_rate_mbps = 1900, .hsfreqrange_code = 0x2F},
 161         {.default_bit_rate_mbps = 1950, .hsfreqrange_code = 0x3F},
 162         {.default_bit_rate_mbps = 2000, .hsfreqrange_code = 0x0F},
 163         {.default_bit_rate_mbps = 2050, .hsfreqrange_code = 0x40},
 164         {.default_bit_rate_mbps = 2100, .hsfreqrange_code = 0x41},
 165         {.default_bit_rate_mbps = 2150, .hsfreqrange_code = 0x42},
 166         {.default_bit_rate_mbps = 2200, .hsfreqrange_code = 0x43},
 167         {.default_bit_rate_mbps = 2250, .hsfreqrange_code = 0x44},
 168         {.default_bit_rate_mbps = 2300, .hsfreqrange_code = 0x45},
 169         {.default_bit_rate_mbps = 2350, .hsfreqrange_code = 0x46},
 170         {.default_bit_rate_mbps = 2400, .hsfreqrange_code = 0x47},
 171         {.default_bit_rate_mbps = 2450, .hsfreqrange_code = 0x48},
 172         {.default_bit_rate_mbps = 2500, .hsfreqrange_code = 0x49}
 173 };
 174
 175 static void kmb_dsi_clk_disable(struct kmb_dsi *kmb_dsi)
 176 {
 177         clk_disable_unprepare(kmb_dsi->clk_mipi);
 178         clk_disable_unprepare(kmb_dsi->clk_mipi_ecfg);
 179         clk_disable_unprepare(kmb_dsi->clk_mipi_cfg);
 180 }
 181
 182 void kmb_dsi_host_unregister(struct kmb_dsi *kmb_dsi)
 183 {
 184         kmb_dsi_clk_disable(kmb_dsi);
 185         mipi_dsi_host_unregister(kmb_dsi->host);
 186 }
 187
 188 /*
 189  * This DSI can only be paired with bridges that do config through i2c
 190  * which is ADV 7535 in the KMB EVM
 191  */
 192 static ssize_t kmb_dsi_host_transfer(struct mipi_dsi_host *host,
 193                                      const struct mipi_dsi_msg *msg)
 194 {
 195         return 0;
 196 }
 197
 198 static int kmb_dsi_host_attach(struct mipi_dsi_host *host,
 199                                struct mipi_dsi_device *dev)
 200 {
 201         return 0;
 202 }
 203
 204 static int kmb_dsi_host_detach(struct mipi_dsi_host *host,
 205                                struct mipi_dsi_device *dev)
 206 {
 207         return 0;
 208 }
 209
 210 static const struct mipi_dsi_host_ops kmb_dsi_host_ops = {
 211         .attach = kmb_dsi_host_attach,
 212         .detach = kmb_dsi_host_detach,
 213         .transfer = kmb_dsi_host_transfer,
 214 };
 215
 216 int kmb_dsi_host_bridge_init(struct device *dev)
 217 {
 218         struct device_node *encoder_node, *dsi_out;
 219
 220         /* Create and register MIPI DSI host */
 221         if (!dsi_host) {
 222                 dsi_host = kzalloc(sizeof(*dsi_host), GFP_KERNEL);
 223                 if (!dsi_host)
 224                         return -ENOMEM;
 225
 226                 dsi_host->ops = &kmb_dsi_host_ops;
 227
 228                 if (!dsi_device) {
 229                         dsi_device = kzalloc(sizeof(*dsi_device), GFP_KERNEL);
 230                         if (!dsi_device) {
 231                                 kfree(dsi_host);
 232                                 return -ENOMEM;
 233                         }
 234                 }
 235
 236                 dsi_host->dev = dev;
 237                 mipi_dsi_host_register(dsi_host);
 238         }
 239
 240         /* Find ADV7535 node and initialize it */
 241         dsi_out = of_graph_get_endpoint_by_regs(dev->of_node, 0, 1);
 242         if (!dsi_out) {
 243                 DRM_ERROR("Failed to get dsi_out node info from DT\n");
 244                 return -EINVAL;
 245         }
 246         encoder_node = of_graph_get_remote_port_parent(dsi_out);
 247         if (!encoder_node) {
 248                 of_node_put(dsi_out);
 249                 DRM_ERROR("Failed to get bridge info from DT\n");
 250                 return -EINVAL;
 251         }
 252         /* Locate drm bridge from the hdmi encoder DT node */
 253         adv_bridge = of_drm_find_bridge(encoder_node);
 254         of_node_put(dsi_out);
 255         of_node_put(encoder_node);
 256         if (!adv_bridge) {
 257                 DRM_DEBUG("Wait for external bridge driver DT\n");
 258                 return -EPROBE_DEFER;
 259         }
 260
 261         return 0;
 262 }
 263
 264 static u32 mipi_get_datatype_params(u32 data_type, u32 data_mode,
 265                                     struct mipi_data_type_params *params)
 266 {
 267         struct mipi_data_type_params data_type_param;
 268
 269         switch (data_type) {
 270         case DSI_LP_DT_PPS_YCBCR420_12B:
 271                 data_type_param.size_constraint_pixels = 2;
 272                 data_type_param.size_constraint_bytes = 3;
 273                 switch (data_mode) {
 274                         /* Case 0 not supported according to MDK */
 275                 case 1:
 276                 case 2:
 277                 case 3:
 278                         data_type_param.pixels_per_pclk = 2;
 279                         data_type_param.bits_per_pclk = 24;
 280                         break;
 281                 default:
 282                         DRM_ERROR("DSI: Invalid data_mode %d\n", data_mode);
 283                         return -EINVAL;
 284                 }
 285                 break;
 286         case DSI_LP_DT_PPS_YCBCR422_16B:
 287                 data_type_param.size_constraint_pixels = 2;
 288                 data_type_param.size_constraint_bytes = 4;
 289                 switch (data_mode) {
 290                         /* Case 0 and 1 not supported according
 291                          * to MDK
 292                          */
 293                 case 2:
 294                         data_type_param.pixels_per_pclk = 1;
 295                         data_type_param.bits_per_pclk = 16;
 296                         break;
 297                 case 3:
 298                         data_type_param.pixels_per_pclk = 2;
 299                         data_type_param.bits_per_pclk = 32;
 300                         break;
 301                 default:
 302                         DRM_ERROR("DSI: Invalid data_mode %d\n", data_mode);
 303                         return -EINVAL;
 304                 }
 305                 break;
 306         case DSI_LP_DT_LPPS_YCBCR422_20B:
 307         case DSI_LP_DT_PPS_YCBCR422_24B:
 308                 data_type_param.size_constraint_pixels = 2;
 309                 data_type_param.size_constraint_bytes = 6;
 310                 switch (data_mode) {
 311                         /* Case 0 not supported according to MDK */
 312                 case 1:
 313                 case 2:
 314                 case 3:
 315                         data_type_param.pixels_per_pclk = 1;
 316                         data_type_param.bits_per_pclk = 24;
 317                         break;
 318                 default:
 319                         DRM_ERROR("DSI: Invalid data_mode %d\n", data_mode);
 320                         return -EINVAL;
 321                 }
 322                 break;
 323         case DSI_LP_DT_PPS_RGB565_16B:
 324                 data_type_param.size_constraint_pixels = 1;
 325                 data_type_param.size_constraint_bytes = 2;
 326                 switch (data_mode) {
 327                 case 0:
 328                 case 1:
 329                         data_type_param.pixels_per_pclk = 1;
 330                         data_type_param.bits_per_pclk = 16;
 331                         break;
 332                 case 2:
 333                 case 3:
 334                         data_type_param.pixels_per_pclk = 2;
 335                         data_type_param.bits_per_pclk = 32;
 336                         break;
 337                 default:
 338                         DRM_ERROR("DSI: Invalid data_mode %d\n", data_mode);
 339                         return -EINVAL;
 340                 }
 341                 break;
 342         case DSI_LP_DT_PPS_RGB666_18B:
 343                 data_type_param.size_constraint_pixels = 4;
 344                 data_type_param.size_constraint_bytes = 9;
 345                 data_type_param.bits_per_pclk = 18;
 346                 data_type_param.pixels_per_pclk = 1;
 347                 break;
 348         case DSI_LP_DT_LPPS_RGB666_18B:
 349         case DSI_LP_DT_PPS_RGB888_24B:
 350                 data_type_param.size_constraint_pixels = 1;
 351                 data_type_param.size_constraint_bytes = 3;
 352                 data_type_param.bits_per_pclk = 24;
 353                 data_type_param.pixels_per_pclk = 1;
 354                 break;
 355         case DSI_LP_DT_PPS_RGB101010_30B:
 356                 data_type_param.size_constraint_pixels = 4;
 357                 data_type_param.size_constraint_bytes = 15;
 358                 data_type_param.bits_per_pclk = 30;
 359                 data_type_param.pixels_per_pclk = 1;
 360                 break;
 361         default:
 362                 DRM_ERROR("DSI: Invalid data_type %d\n", data_type);
 363                 return -EINVAL;
 364         }
 365
 366         *params = data_type_param;
 367         return 0;
 368 }
 369
 370 static u32 compute_wc(u32 width_px, u8 size_constr_p, u8 size_constr_b)
 371 {
 372         /* Calculate the word count for each long packet */
 373         return (((width_px / size_constr_p) * size_constr_b) & 0xffff);
 374 }
 375
 376 static u32 compute_unpacked_bytes(u32 wc, u8 bits_per_pclk)
 377 {
 378         /* Number of PCLK cycles needed to transfer a line
 379          * with each PCLK cycle, 4 Bytes are sent through the PPL module
 380          */
 381         return ((wc * 8) / bits_per_pclk) * 4;
 382 }
 383
 384 static u32 mipi_tx_fg_section_cfg_regs(struct kmb_dsi *kmb_dsi,
 385                                        u8 frame_id, u8 section,
 386                                        u32 height_lines, u32 unpacked_bytes,
 387                                        struct mipi_tx_frame_sect_phcfg *ph_cfg)
 388 {
 389         u32 cfg = 0;
 390         u32 ctrl_no = MIPI_CTRL6;
 391         u32 reg_adr;
 392
 393         /* Frame section packet header */
 394         /* Word count bits [15:0] */
 395         cfg = (ph_cfg->wc & MIPI_TX_SECT_WC_MASK) << 0;
 396
 397         /* Data type (bits [21:16]) */
 398         cfg |= ((ph_cfg->data_type & MIPI_TX_SECT_DT_MASK)
 399                 << MIPI_TX_SECT_DT_SHIFT);
 400
 401         /* Virtual channel (bits [23:22]) */
 402         cfg |= ((ph_cfg->vchannel & MIPI_TX_SECT_VC_MASK)
 403                 << MIPI_TX_SECT_VC_SHIFT);
 404
 405         /* Data mode (bits [24:25]) */
 406         cfg |= ((ph_cfg->data_mode & MIPI_TX_SECT_DM_MASK)
 407                 << MIPI_TX_SECT_DM_SHIFT);
 408         if (ph_cfg->dma_packed)
 409                 cfg |= MIPI_TX_SECT_DMA_PACKED;
 410
 411         dev_dbg(kmb_dsi->dev,
 412                 "ctrl=%d frame_id=%d section=%d cfg=%x packed=%d\n",
 413                   ctrl_no, frame_id, section, cfg, ph_cfg->dma_packed);
 414         kmb_write_mipi(kmb_dsi,
 415                        (MIPI_TXm_HS_FGn_SECTo_PH(ctrl_no, frame_id, section)),
 416                        cfg);
 417
 418         /* Unpacked bytes */
 419
 420         /* There are 4 frame generators and each fg has 4 sections
 421          * There are 2 registers for unpacked bytes (# bytes each
 422          * section occupies in memory)
 423          * REG_UNPACKED_BYTES0: [15:0]-BYTES0, [31:16]-BYTES1
 424          * REG_UNPACKED_BYTES1: [15:0]-BYTES2, [31:16]-BYTES3
 425          */
 426         reg_adr =
 427             MIPI_TXm_HS_FGn_SECT_UNPACKED_BYTES0(ctrl_no,
 428                                                  frame_id) + (section / 2) * 4;
 429         kmb_write_bits_mipi(kmb_dsi, reg_adr, (section % 2) * 16, 16,
 430                             unpacked_bytes);
 431         dev_dbg(kmb_dsi->dev,
 432                 "unpacked_bytes = %d, wordcount = %d\n", unpacked_bytes,
 433                   ph_cfg->wc);
 434
 435         /* Line config */
 436         reg_adr = MIPI_TXm_HS_FGn_SECTo_LINE_CFG(ctrl_no, frame_id, section);
 437         kmb_write_mipi(kmb_dsi, reg_adr, height_lines);
 438         return 0;
 439 }
 440
 441 static u32 mipi_tx_fg_section_cfg(struct kmb_dsi *kmb_dsi,
 442                                   u8 frame_id, u8 section,
 443                                   struct mipi_tx_frame_section_cfg *frame_scfg,
 444                                   u32 *bits_per_pclk, u32 *wc)
 445 {
 446         u32 ret = 0;
 447         u32 unpacked_bytes;
 448         struct mipi_data_type_params data_type_parameters;
 449         struct mipi_tx_frame_sect_phcfg ph_cfg;
 450
 451         ret = mipi_get_datatype_params(frame_scfg->data_type,
 452                                        frame_scfg->data_mode,
 453                                        &data_type_parameters);
 454         if (ret)
 455                 return ret;
 456
 457         /* Packet width has to be a multiple of the minimum packet width
 458          * (in pixels) set for each data type
 459          */
 460         if (frame_scfg->width_pixels %
 461             data_type_parameters.size_constraint_pixels != 0)
 462                 return -EINVAL;
 463
 464         *wc = compute_wc(frame_scfg->width_pixels,
 465                          data_type_parameters.size_constraint_pixels,
 466                          data_type_parameters.size_constraint_bytes);
 467         unpacked_bytes = compute_unpacked_bytes(*wc,
 468                                                 data_type_parameters.bits_per_pclk);
 469         ph_cfg.wc = *wc;
 470         ph_cfg.data_mode = frame_scfg->data_mode;
 471         ph_cfg.data_type = frame_scfg->data_type;
 472         ph_cfg.dma_packed = frame_scfg->dma_packed;
 473         ph_cfg.vchannel = frame_id;
 474
 475         mipi_tx_fg_section_cfg_regs(kmb_dsi, frame_id, section,
 476                                     frame_scfg->height_lines,
 477                                     unpacked_bytes, &ph_cfg);
 478
 479         /* Caller needs bits_per_clk for additional caluclations */
 480         *bits_per_pclk = data_type_parameters.bits_per_pclk;
 481
 482         return 0;
 483 }
 484
 485 static void mipi_tx_fg_cfg_regs(struct kmb_dsi *kmb_dsi, u8 frame_gen,
 486                                 struct mipi_tx_frame_timing_cfg *fg_cfg)
 487 {
 488         u32 sysclk;
 489         u32 ppl_llp_ratio;
 490         u32 ctrl_no = MIPI_CTRL6, reg_adr, val, offset;
 491
 492         /* 500 Mhz system clock minus 50 to account for the difference in
 493          * MIPI clock speed in RTL tests
 494          */
 495         sysclk = kmb_dsi->sys_clk_mhz - 50;
 496
 497         /* PPL-Pixel Packing Layer, LLP-Low Level Protocol
 498          * Frame genartor timing parameters are clocked on the system clock,
 499          * whereas as the equivalent parameters in the LLP blocks are clocked
 500          * on LLP Tx clock from the D-PHY - BYTE clock
 501          */
 502
 503         /* Multiply by 1000 to maintain precision */
 504         ppl_llp_ratio = ((fg_cfg->bpp / 8) * sysclk * 1000) /
 505             ((fg_cfg->lane_rate_mbps / 8) * fg_cfg->active_lanes);
 506
 507         dev_dbg(kmb_dsi->dev, "ppl_llp_ratio=%d\n", ppl_llp_ratio);
 508         dev_dbg(kmb_dsi->dev, "bpp=%d sysclk=%d lane-rate=%d active-lanes=%d\n",
 509                 fg_cfg->bpp, sysclk, fg_cfg->lane_rate_mbps,
 510                  fg_cfg->active_lanes);
 511
 512         /* Frame generator number of lines */
 513         reg_adr = MIPI_TXm_HS_FGn_NUM_LINES(ctrl_no, frame_gen);
 514         kmb_write_mipi(kmb_dsi, reg_adr, fg_cfg->v_active);
 515
 516         /* vsync width
 517          * There are 2 registers for vsync width (VSA in lines for
 518          * channels 0-3)
 519          * REG_VSYNC_WIDTH0: [15:0]-VSA for channel0, [31:16]-VSA for channel1
 520          * REG_VSYNC_WIDTH1: [15:0]-VSA for channel2, [31:16]-VSA for channel3
 521          */
 522         offset = (frame_gen % 2) * 16;
 523         reg_adr = MIPI_TXm_HS_VSYNC_WIDTHn(ctrl_no, frame_gen / 2);
 524         kmb_write_bits_mipi(kmb_dsi, reg_adr, offset, 16, fg_cfg->vsync_width);
 525
 526         /* vertical backporch (vbp) */
 527         reg_adr = MIPI_TXm_HS_V_BACKPORCHESn(ctrl_no, frame_gen / 2);
 528         kmb_write_bits_mipi(kmb_dsi, reg_adr, offset, 16, fg_cfg->v_backporch);
 529
 530         /* vertical frontporch (vfp) */
 531         reg_adr = MIPI_TXm_HS_V_FRONTPORCHESn(ctrl_no, frame_gen / 2);
 532         kmb_write_bits_mipi(kmb_dsi, reg_adr, offset, 16, fg_cfg->v_frontporch);
 533
 534         /* vertical active (vactive) */
 535         reg_adr = MIPI_TXm_HS_V_ACTIVEn(ctrl_no, frame_gen / 2);
 536         kmb_write_bits_mipi(kmb_dsi, reg_adr, offset, 16, fg_cfg->v_active);
 537
 538         /* hsync width */
 539         reg_adr = MIPI_TXm_HS_HSYNC_WIDTHn(ctrl_no, frame_gen);
 540         kmb_write_mipi(kmb_dsi, reg_adr,
 541                        (fg_cfg->hsync_width * ppl_llp_ratio) / 1000);
 542
 543         /* horizontal backporch (hbp) */
 544         reg_adr = MIPI_TXm_HS_H_BACKPORCHn(ctrl_no, frame_gen);
 545         kmb_write_mipi(kmb_dsi, reg_adr,
 546                        (fg_cfg->h_backporch * ppl_llp_ratio) / 1000);
 547
 548         /* horizontal frontporch (hfp) */
 549         reg_adr = MIPI_TXm_HS_H_FRONTPORCHn(ctrl_no, frame_gen);
 550         kmb_write_mipi(kmb_dsi, reg_adr,
 551                        (fg_cfg->h_frontporch * ppl_llp_ratio) / 1000);
 552
 553         /* horizontal active (ha) */
 554         reg_adr = MIPI_TXm_HS_H_ACTIVEn(ctrl_no, frame_gen);
 555
 556         /* convert h_active which is wc in bytes to cycles */
 557         val = (fg_cfg->h_active * sysclk * 1000) /
 558             ((fg_cfg->lane_rate_mbps / 8) * fg_cfg->active_lanes);
 559         val /= 1000;
 560         kmb_write_mipi(kmb_dsi, reg_adr, val);
 561
 562         /* llp hsync width */
 563         reg_adr = MIPI_TXm_HS_LLP_HSYNC_WIDTHn(ctrl_no, frame_gen);
 564         kmb_write_mipi(kmb_dsi, reg_adr, fg_cfg->hsync_width * (fg_cfg->bpp / 8));
 565
 566         /* llp h backporch */
 567         reg_adr = MIPI_TXm_HS_LLP_H_BACKPORCHn(ctrl_no, frame_gen);
 568         kmb_write_mipi(kmb_dsi, reg_adr, fg_cfg->h_backporch * (fg_cfg->bpp / 8));
 569
 570         /* llp h frontporch */
 571         reg_adr = MIPI_TXm_HS_LLP_H_FRONTPORCHn(ctrl_no, frame_gen);
 572         kmb_write_mipi(kmb_dsi, reg_adr,
 573                        fg_cfg->h_frontporch * (fg_cfg->bpp / 8));
 574 }
 575
 576 static void mipi_tx_fg_cfg(struct kmb_dsi *kmb_dsi, u8 frame_gen,
 577                            u8 active_lanes, u32 bpp, u32 wc,
 578                            u32 lane_rate_mbps, struct mipi_tx_frame_cfg *fg_cfg)
 579 {
 580         u32 i, fg_num_lines = 0;
 581         struct mipi_tx_frame_timing_cfg fg_t_cfg;
 582
 583         /* Calculate the total frame generator number of
 584          * lines based on it's active sections
 585          */
 586         for (i = 0; i < MIPI_TX_FRAME_GEN_SECTIONS; i++) {
 587                 if (fg_cfg->sections[i])
 588                         fg_num_lines += fg_cfg->sections[i]->height_lines;
 589         }
 590
 591         fg_t_cfg.bpp = bpp;
 592         fg_t_cfg.lane_rate_mbps = lane_rate_mbps;
 593         fg_t_cfg.hsync_width = fg_cfg->hsync_width;
 594         fg_t_cfg.h_backporch = fg_cfg->h_backporch;
 595         fg_t_cfg.h_frontporch = fg_cfg->h_frontporch;
 596         fg_t_cfg.h_active = wc;
 597         fg_t_cfg.vsync_width = fg_cfg->vsync_width;
 598         fg_t_cfg.v_backporch = fg_cfg->v_backporch;
 599         fg_t_cfg.v_frontporch = fg_cfg->v_frontporch;
 600         fg_t_cfg.v_active = fg_num_lines;
 601         fg_t_cfg.active_lanes = active_lanes;
 602
 603         /* Apply frame generator timing setting */
 604         mipi_tx_fg_cfg_regs(kmb_dsi, frame_gen, &fg_t_cfg);
 605 }
 606
 607 static void mipi_tx_multichannel_fifo_cfg(struct kmb_dsi *kmb_dsi,
 608                                           u8 active_lanes, u8 vchannel_id)
 609 {
 610         u32 fifo_size, fifo_rthreshold;
 611         u32 ctrl_no = MIPI_CTRL6;
 612
 613         /* Clear all mc fifo channel sizes and thresholds */
 614         kmb_write_mipi(kmb_dsi, MIPI_TX_HS_MC_FIFO_CTRL_EN, 0);
 615         kmb_write_mipi(kmb_dsi, MIPI_TX_HS_MC_FIFO_CHAN_ALLOC0, 0);
 616         kmb_write_mipi(kmb_dsi, MIPI_TX_HS_MC_FIFO_CHAN_ALLOC1, 0);
 617         kmb_write_mipi(kmb_dsi, MIPI_TX_HS_MC_FIFO_RTHRESHOLD0, 0);
 618         kmb_write_mipi(kmb_dsi, MIPI_TX_HS_MC_FIFO_RTHRESHOLD1, 0);
 619
 620         fifo_size = ((active_lanes > MIPI_D_LANES_PER_DPHY) ?
 621                      MIPI_CTRL_4LANE_MAX_MC_FIFO_LOC :
 622                      MIPI_CTRL_2LANE_MAX_MC_FIFO_LOC) - 1;
 623
 624         /* MC fifo size for virtual channels 0-3
 625          * REG_MC_FIFO_CHAN_ALLOC0: [8:0]-channel0, [24:16]-channel1
 626          * REG_MC_FIFO_CHAN_ALLOC1: [8:0]-2, [24:16]-channel3
 627          */
 628         SET_MC_FIFO_CHAN_ALLOC(kmb_dsi, ctrl_no, vchannel_id, fifo_size);
 629
 630         /* Set threshold to half the fifo size, actual size=size*16 */
 631         fifo_rthreshold = ((fifo_size) * 8) & BIT_MASK_16;
 632         SET_MC_FIFO_RTHRESHOLD(kmb_dsi, ctrl_no, vchannel_id, fifo_rthreshold);
 633
 634         /* Enable the MC FIFO channel corresponding to the Virtual Channel */
 635         kmb_set_bit_mipi(kmb_dsi, MIPI_TXm_HS_MC_FIFO_CTRL_EN(ctrl_no),
 636                          vchannel_id);
 637 }
 638
 639 static void mipi_tx_ctrl_cfg(struct kmb_dsi *kmb_dsi, u8 fg_id,
 640                              struct mipi_ctrl_cfg *ctrl_cfg)
 641 {
 642         u32 sync_cfg = 0, ctrl = 0, fg_en;
 643         u32 ctrl_no = MIPI_CTRL6;
 644
 645         /* MIPI_TX_HS_SYNC_CFG */
 646         if (ctrl_cfg->tx_ctrl_cfg.line_sync_pkt_en)
 647                 sync_cfg |= LINE_SYNC_PKT_ENABLE;
 648         if (ctrl_cfg->tx_ctrl_cfg.frame_counter_active)
 649                 sync_cfg |= FRAME_COUNTER_ACTIVE;
 650         if (ctrl_cfg->tx_ctrl_cfg.line_counter_active)
 651                 sync_cfg |= LINE_COUNTER_ACTIVE;
 652         if (ctrl_cfg->tx_ctrl_cfg.tx_dsi_cfg->v_blanking)
 653                 sync_cfg |= DSI_V_BLANKING;
 654         if (ctrl_cfg->tx_ctrl_cfg.tx_dsi_cfg->hsa_blanking)
 655                 sync_cfg |= DSI_HSA_BLANKING;
 656         if (ctrl_cfg->tx_ctrl_cfg.tx_dsi_cfg->hbp_blanking)
 657                 sync_cfg |= DSI_HBP_BLANKING;
 658         if (ctrl_cfg->tx_ctrl_cfg.tx_dsi_cfg->hfp_blanking)
 659                 sync_cfg |= DSI_HFP_BLANKING;
 660         if (ctrl_cfg->tx_ctrl_cfg.tx_dsi_cfg->sync_pulse_eventn)
 661                 sync_cfg |= DSI_SYNC_PULSE_EVENTN;
 662         if (ctrl_cfg->tx_ctrl_cfg.tx_dsi_cfg->lpm_first_vsa_line)
 663                 sync_cfg |= DSI_LPM_FIRST_VSA_LINE;
 664         if (ctrl_cfg->tx_ctrl_cfg.tx_dsi_cfg->lpm_last_vfp_line)
 665                 sync_cfg |= DSI_LPM_LAST_VFP_LINE;
 666
 667         /* Enable frame generator */
 668         fg_en = 1 << fg_id;
 669         sync_cfg |= FRAME_GEN_EN(fg_en);
 670
 671         if (ctrl_cfg->tx_ctrl_cfg.tx_always_use_hact)
 672                 sync_cfg |= ALWAYS_USE_HACT(fg_en);
 673         if (ctrl_cfg->tx_ctrl_cfg.tx_hact_wait_stop)
 674                 sync_cfg |= HACT_WAIT_STOP(fg_en);
 675
 676         dev_dbg(kmb_dsi->dev, "sync_cfg=%d fg_en=%d\n", sync_cfg, fg_en);
 677
 678         /* MIPI_TX_HS_CTRL */
 679
 680         /* type:DSI, source:LCD */
 681         ctrl = HS_CTRL_EN | TX_SOURCE;
 682         ctrl |= LCD_VC(fg_id);
 683         ctrl |= ACTIVE_LANES(ctrl_cfg->active_lanes - 1);
 684         if (ctrl_cfg->tx_ctrl_cfg.tx_dsi_cfg->eotp_en)
 685                 ctrl |= DSI_EOTP_EN;
 686         if (ctrl_cfg->tx_ctrl_cfg.tx_dsi_cfg->hfp_blank_en)
 687                 ctrl |= DSI_CMD_HFP_EN;
 688
 689         /*67 ns stop time */
 690         ctrl |= HSEXIT_CNT(0x43);
 691
 692         kmb_write_mipi(kmb_dsi, MIPI_TXm_HS_SYNC_CFG(ctrl_no), sync_cfg);
 693         kmb_write_mipi(kmb_dsi, MIPI_TXm_HS_CTRL(ctrl_no), ctrl);
 694 }
 695
 696 static u32 mipi_tx_init_cntrl(struct kmb_dsi *kmb_dsi,
 697                               struct mipi_ctrl_cfg *ctrl_cfg)
 698 {
 699         u32 ret = 0;
 700         u8 active_vchannels = 0;
 701         u8 frame_id, sect;
 702         u32 bits_per_pclk = 0;
 703         u32 word_count = 0;
 704         struct mipi_tx_frame_cfg *frame;
 705
 706         /* This is the order to initialize MIPI TX:
 707          * 1. set frame section parameters
 708          * 2. set frame specific parameters
 709          * 3. connect lcd to mipi
 710          * 4. multi channel fifo cfg
 711          * 5. set mipitxcctrlcfg
 712          */
 713
 714         for (frame_id = 0; frame_id < 4; frame_id++) {
 715                 frame = ctrl_cfg->tx_ctrl_cfg.frames[frame_id];
 716
 717                 /* Find valid frame, assume only one valid frame */
 718                 if (!frame)
 719                         continue;
 720
 721                 /* Frame Section configuration */
 722                 /* TODO - assume there is only one valid section in a frame,
 723                  * so bits_per_pclk and word_count are only set once
 724                  */
 725                 for (sect = 0; sect < MIPI_CTRL_VIRTUAL_CHANNELS; sect++) {
 726                         if (!frame->sections[sect])
 727                                 continue;
 728
 729                         ret = mipi_tx_fg_section_cfg(kmb_dsi, frame_id, sect,
 730                                                      frame->sections[sect],
 731                                                      &bits_per_pclk,
 732                                                      &word_count);
 733                         if (ret)
 734                                 return ret;
 735                 }
 736
 737                 /* Set frame specific parameters */
 738                 mipi_tx_fg_cfg(kmb_dsi, frame_id, ctrl_cfg->active_lanes,
 739                                bits_per_pclk, word_count,
 740                                ctrl_cfg->lane_rate_mbps, frame);
 741
 742                 active_vchannels++;
 743
 744                 /* Stop iterating as only one virtual channel
 745                  * shall be used for LCD connection
 746                  */
 747                 break;
 748         }
 749
 750         if (active_vchannels == 0)
 751                 return -EINVAL;
 752         /* Multi-Channel FIFO Configuration */
 753         mipi_tx_multichannel_fifo_cfg(kmb_dsi, ctrl_cfg->active_lanes, frame_id);
 754
 755         /* Frame Generator Enable */
 756         mipi_tx_ctrl_cfg(kmb_dsi, frame_id, ctrl_cfg);
 757
 758         return ret;
 759 }
 760
 761 static void test_mode_send(struct kmb_dsi *kmb_dsi, u32 dphy_no,
 762                            u32 test_code, u32 test_data)
 763 {
 764         /* Steps to send test code:
 765          * - set testclk HIGH
 766          * - set testdin with test code
 767          * - set testen HIGH
 768          * - set testclk LOW
 769          * - set testen LOW
 770          */
 771
 772         /* Set testclk high */
 773         SET_DPHY_TEST_CTRL1_CLK(kmb_dsi, dphy_no);
 774
 775         /* Set testdin */
 776         SET_TEST_DIN0_3(kmb_dsi, dphy_no, test_code);
 777
 778         /* Set testen high */
 779         SET_DPHY_TEST_CTRL1_EN(kmb_dsi, dphy_no);
 780
 781         /* Set testclk low */
 782         CLR_DPHY_TEST_CTRL1_CLK(kmb_dsi, dphy_no);
 783
 784         /* Set testen low */
 785         CLR_DPHY_TEST_CTRL1_EN(kmb_dsi, dphy_no);
 786
 787         if (test_code) {
 788                 /*  Steps to send test data:
 789                  * - set testen LOW
 790                  * - set testclk LOW
 791                  * - set testdin with data
 792                  * - set testclk HIGH
 793                  */
 794
 795                 /* Set testen low */
 796                 CLR_DPHY_TEST_CTRL1_EN(kmb_dsi, dphy_no);
 797
 798                 /* Set testclk low */
 799                 CLR_DPHY_TEST_CTRL1_CLK(kmb_dsi, dphy_no);
 800
 801                 /* Set data in testdin */
 802                 kmb_write_mipi(kmb_dsi,
 803                                DPHY_TEST_DIN0_3 + ((dphy_no / 0x4) * 0x4),
 804                                test_data << ((dphy_no % 4) * 8));
 805
 806                 /* Set testclk high */
 807                 SET_DPHY_TEST_CTRL1_CLK(kmb_dsi, dphy_no);
 808         }
 809 }
 810
 811 static inline void
 812         set_test_mode_src_osc_freq_target_low_bits(struct kmb_dsi *kmb_dsi,
 813                                                    u32 dphy_no,
 814                                                    u32 freq)
 815 {
 816         /* Typical rise/fall time=166, refer Table 1207 databook,
 817          * sr_osc_freq_target[7:0]
 818          */
 819         test_mode_send(kmb_dsi, dphy_no, TEST_CODE_SLEW_RATE_DDL_CYCLES,
 820                        (freq & 0x7f));
 821 }
 822
 823 static inline void
 824         set_test_mode_src_osc_freq_target_hi_bits(struct kmb_dsi *kmb_dsi,
 825                                                   u32 dphy_no,
 826                                                   u32 freq)
 827 {
 828         u32 data;
 829
 830         /* Flag this as high nibble */
 831         data = ((freq >> 6) & 0x1f) | (1 << 7);
 832
 833         /* Typical rise/fall time=166, refer Table 1207 databook,
 834          * sr_osc_freq_target[11:7]
 835          */
 836         test_mode_send(kmb_dsi, dphy_no, TEST_CODE_SLEW_RATE_DDL_CYCLES, data);
 837 }
 838
 839 static void mipi_tx_get_vco_params(struct vco_params *vco)
 840 {
 841         int i;
 842
 843         for (i = 0; i < ARRAY_SIZE(vco_table); i++) {
 844                 if (vco->freq < vco_table[i].freq) {
 845                         *vco = vco_table[i];
 846                         return;
 847                 }
 848         }
 849
 850         WARN_ONCE(1, "Invalid vco freq = %u for PLL setup\n", vco->freq);
 851 }
 852
 853 static void mipi_tx_pll_setup(struct kmb_dsi *kmb_dsi, u32 dphy_no,
 854                               u32 ref_clk_mhz, u32 target_freq_mhz)
 855 {
 856         u32 best_n = 0, best_m = 0;
 857         u32 n = 0, m = 0, div = 0, delta, freq = 0, t_freq;
 858         u32 best_freq_delta = 3000;
 859
 860         /* pll_ref_clk: - valid range: 2~64 MHz; Typically 24 MHz
 861          * Fvco: - valid range: 320~1250 MHz (Gen3 D-PHY)
 862          * Fout: - valid range: 40~1250 MHz (Gen3 D-PHY)
 863          * n: - valid range [0 15]
 864          * N: - N = n + 1
 865          *      -valid range: [1 16]
 866          *      -conditions: - (pll_ref_clk / N) >= 2 MHz
 867          *             -(pll_ref_clk / N) <= 8 MHz
 868          * m: valid range [62 623]
 869          * M: - M = m + 2
 870          *      -valid range [64 625]
 871          *      -Fvco = (M/N) * pll_ref_clk
 872          */
 873         struct vco_params vco_p = {
 874                 .range = 0,
 875                 .divider = 1,
 876         };
 877
 878         vco_p.freq = target_freq_mhz;
 879         mipi_tx_get_vco_params(&vco_p);
 880
 881         /* Search pll n parameter */
 882         for (n = PLL_N_MIN; n <= PLL_N_MAX; n++) {
 883                 /* Calculate the pll input frequency division ratio
 884                  * multiply by 1000 for precision -
 885                  * no floating point, add n for rounding
 886                  */
 887                 div = ((ref_clk_mhz * 1000) + n) / (n + 1);
 888
 889                 /* Found a valid n parameter */
 890                 if ((div < 2000 || div > 8000))
 891                         continue;
 892
 893                 /* Search pll m parameter */
 894                 for (m = PLL_M_MIN; m <= PLL_M_MAX; m++) {
 895                         /* Calculate the Fvco(DPHY PLL output frequency)
 896                          * using the current n,m params
 897                          */
 898                         freq = div * (m + 2);
 899                         freq /= 1000;
 900
 901                         /* Trim the potential pll freq to max supported */
 902                         if (freq > PLL_FVCO_MAX)
 903                                 continue;
 904
 905                         delta = abs(freq - target_freq_mhz);
 906
 907                         /* Select the best (closest to target pll freq)
 908                          * n,m parameters so far
 909                          */
 910                         if (delta < best_freq_delta) {
 911                                 best_n = n;
 912                                 best_m = m;
 913                                 best_freq_delta = delta;
 914                         }
 915                 }
 916         }
 917
 918         /* Program vco_cntrl parameter
 919          * PLL_VCO_Control[5:0] = pll_vco_cntrl_ovr,
 920          * PLL_VCO_Control[6]   = pll_vco_cntrl_ovr_en
 921          */
 922         test_mode_send(kmb_dsi, dphy_no, TEST_CODE_PLL_VCO_CTRL, (vco_p.range
 923                                                                 | (1 << 6)));
 924
 925         /* Program m, n pll parameters */
 926         dev_dbg(kmb_dsi->dev, "m = %d n = %d\n", best_m, best_n);
 927
 928         /* PLL_Input_Divider_Ratio[3:0] = pll_n_ovr */
 929         test_mode_send(kmb_dsi, dphy_no, TEST_CODE_PLL_INPUT_DIVIDER,
 930                        (best_n & 0x0f));
 931
 932         /* m - low nibble PLL_Loop_Divider_Ratio[4:0]
 933          * pll_m_ovr[4:0]
 934          */
 935         test_mode_send(kmb_dsi, dphy_no, TEST_CODE_PLL_FEEDBACK_DIVIDER,
 936                        (best_m & 0x1f));
 937
 938         /* m - high nibble PLL_Loop_Divider_Ratio[4:0]
 939          * pll_m_ovr[9:5]
 940          */
 941         test_mode_send(kmb_dsi, dphy_no, TEST_CODE_PLL_FEEDBACK_DIVIDER,
 942                        ((best_m >> 5) & 0x1f) | PLL_FEEDBACK_DIVIDER_HIGH);
 943
 944         /* Enable overwrite of n,m parameters :pll_n_ovr_en, pll_m_ovr_en */
 945         test_mode_send(kmb_dsi, dphy_no, TEST_CODE_PLL_OUTPUT_CLK_SEL,
 946                        (PLL_N_OVR_EN | PLL_M_OVR_EN));
 947
 948         /* Program Charge-Pump parameters */
 949
 950         /* pll_prop_cntrl-fixed values for prop_cntrl from DPHY doc */
 951         t_freq = target_freq_mhz * vco_p.divider;
 952         test_mode_send(kmb_dsi, dphy_no,
 953                        TEST_CODE_PLL_PROPORTIONAL_CHARGE_PUMP_CTRL,
 954                        ((t_freq > 1150) ? 0x0C : 0x0B));
 955
 956         /* pll_int_cntrl-fixed value for int_cntrl from DPHY doc */
 957         test_mode_send(kmb_dsi, dphy_no, TEST_CODE_PLL_INTEGRAL_CHARGE_PUMP_CTRL,
 958                        0x00);
 959
 960         /* pll_gmp_cntrl-fixed value for gmp_cntrl from DPHY doci */
 961         test_mode_send(kmb_dsi, dphy_no, TEST_CODE_PLL_GMP_CTRL, 0x10);
 962
 963         /* pll_cpbias_cntrl-fixed value for cpbias_cntrl from DPHY doc */
 964         test_mode_send(kmb_dsi, dphy_no, TEST_CODE_PLL_CHARGE_PUMP_BIAS, 0x10);
 965
 966         /* pll_th1 -Lock Detector Phase error threshold,
 967          * document gives fixed value
 968          */
 969         test_mode_send(kmb_dsi, dphy_no, TEST_CODE_PLL_PHASE_ERR_CTRL, 0x02);
 970
 971         /* PLL Lock Configuration */
 972
 973         /* pll_th2 - Lock Filter length, document gives fixed value */
 974         test_mode_send(kmb_dsi, dphy_no, TEST_CODE_PLL_LOCK_FILTER, 0x60);
 975
 976         /* pll_th3- PLL Unlocking filter, document gives fixed value */
 977         test_mode_send(kmb_dsi, dphy_no, TEST_CODE_PLL_UNLOCK_FILTER, 0x03);
 978
 979         /* pll_lock_sel-PLL Lock Detector Selection,
 980          * document gives fixed value
 981          */
 982         test_mode_send(kmb_dsi, dphy_no, TEST_CODE_PLL_LOCK_DETECTOR, 0x02);
 983 }
 984
 985 static void set_slewrate_gt_1500(struct kmb_dsi *kmb_dsi, u32 dphy_no)
 986 {
 987         u32 test_code = 0, test_data = 0;
 988         /* Bypass slew rate calibration algorithm
 989          * bits[1:0} srcal_en_ovr_en, srcal_en_ovr
 990          */
 991         test_code = TEST_CODE_SLEW_RATE_OVERRIDE_CTRL;
 992         test_data = 0x02;
 993         test_mode_send(kmb_dsi, dphy_no, test_code, test_data);
 994
 995         /* Disable slew rate calibration */
 996         test_code = TEST_CODE_SLEW_RATE_DDL_LOOP_CTRL;
 997         test_data = 0x00;
 998         test_mode_send(kmb_dsi, dphy_no, test_code, test_data);
 999 }
1000
1001 static void set_slewrate_gt_1000(struct kmb_dsi *kmb_dsi, u32 dphy_no)
1002 {
1003         u32 test_code = 0, test_data = 0;
1004
1005         /* BitRate: > 1 Gbps && <= 1.5 Gbps: - slew rate control ON
1006          * typical rise/fall times: 166 ps
1007          */
1008
1009         /* Do not bypass slew rate calibration algorithm
1010          * bits[1:0}=srcal_en_ovr_en, srcal_en_ovr, bit[6]=sr_range
1011          */
1012         test_code = TEST_CODE_SLEW_RATE_OVERRIDE_CTRL;
1013         test_data = (0x03 | (1 << 6));
1014         test_mode_send(kmb_dsi, dphy_no, test_code, test_data);
1015
1016         /* Enable slew rate calibration */
1017         test_code = TEST_CODE_SLEW_RATE_DDL_LOOP_CTRL;
1018         test_data = 0x01;
1019         test_mode_send(kmb_dsi, dphy_no, test_code, test_data);
1020
1021         /* Set sr_osc_freq_target[6:0] low nibble
1022          * typical rise/fall time=166, refer Table 1207 databook
1023          */
1024         test_code = TEST_CODE_SLEW_RATE_DDL_CYCLES;
1025         test_data = (0x72f & 0x7f);
1026         test_mode_send(kmb_dsi, dphy_no, test_code, test_data);
1027
1028         /* Set sr_osc_freq_target[11:7] high nibble
1029          * Typical rise/fall time=166, refer Table 1207 databook
1030          */
1031         test_code = TEST_CODE_SLEW_RATE_DDL_CYCLES;
1032         test_data = ((0x72f >> 6) & 0x1f) | (1 << 7);
1033         test_mode_send(kmb_dsi, dphy_no, test_code, test_data);
1034 }
1035
1036 static void set_slewrate_lt_1000(struct kmb_dsi *kmb_dsi, u32 dphy_no)
1037 {
1038         u32 test_code = 0, test_data = 0;
1039
1040         /* lane_rate_mbps <= 1000 Mbps
1041          * BitRate:  <= 1 Gbps:
1042          * - slew rate control ON
1043          * - typical rise/fall times: 225 ps
1044          */
1045
1046         /* Do not bypass slew rate calibration algorithm */
1047         test_code = TEST_CODE_SLEW_RATE_OVERRIDE_CTRL;
1048         test_data = (0x03 | (1 << 6));
1049         test_mode_send(kmb_dsi, dphy_no, test_code, test_data);
1050
1051         /* Enable slew rate calibration */
1052         test_code = TEST_CODE_SLEW_RATE_DDL_LOOP_CTRL;
1053         test_data = 0x01;
1054         test_mode_send(kmb_dsi, dphy_no, test_code, test_data);
1055
1056         /* Typical rise/fall time=255, refer Table 1207 databook */
1057         test_code = TEST_CODE_SLEW_RATE_DDL_CYCLES;
1058         test_data = (0x523 & 0x7f);
1059         test_mode_send(kmb_dsi, dphy_no, test_code, test_data);
1060
1061         /* Set sr_osc_freq_target[11:7] high nibble */
1062         test_code = TEST_CODE_SLEW_RATE_DDL_CYCLES;
1063         test_data = ((0x523 >> 6) & 0x1f) | (1 << 7);
1064         test_mode_send(kmb_dsi, dphy_no, test_code, test_data);
1065 }
1066
1067 static void setup_pll(struct kmb_dsi *kmb_dsi, u32 dphy_no,
1068                       struct mipi_ctrl_cfg *cfg)
1069 {
1070         u32 test_code = 0, test_data = 0;
1071
1072         /* Set PLL regulator in bypass */
1073         test_code = TEST_CODE_PLL_ANALOG_PROG;
1074         test_data = 0x01;
1075         test_mode_send(kmb_dsi, dphy_no, test_code, test_data);
1076
1077         /* PLL Parameters Setup */
1078         mipi_tx_pll_setup(kmb_dsi, dphy_no, cfg->ref_clk_khz / 1000,
1079                           cfg->lane_rate_mbps / 2);
1080
1081         /* Set clksel */
1082         kmb_write_bits_mipi(kmb_dsi, DPHY_INIT_CTRL1, PLL_CLKSEL_0, 2, 0x01);
1083
1084         /* Set pll_shadow_control */
1085         kmb_set_bit_mipi(kmb_dsi, DPHY_INIT_CTRL1, PLL_SHADOW_CTRL);
1086 }
1087
1088 static void set_lane_data_rate(struct kmb_dsi *kmb_dsi, u32 dphy_no,
1089                                struct mipi_ctrl_cfg *cfg)
1090 {
1091         u32 i, test_code = 0, test_data = 0;
1092
1093         for (i = 0; i < MIPI_DPHY_DEFAULT_BIT_RATES; i++) {
1094                 if (mipi_hs_freq_range[i].default_bit_rate_mbps <
1095                     cfg->lane_rate_mbps)
1096                         continue;
1097
1098                 /* Send the test code and data */
1099                 /* bit[6:0] = hsfreqrange_ovr bit[7] = hsfreqrange_ovr_en */
1100                 test_code = TEST_CODE_HS_FREQ_RANGE_CFG;
1101                 test_data = (mipi_hs_freq_range[i].hsfreqrange_code & 0x7f) |
1102                     (1 << 7);
1103                 test_mode_send(kmb_dsi, dphy_no, test_code, test_data);
1104                 break;
1105         }
1106 }
1107
1108 static void dphy_init_sequence(struct kmb_dsi *kmb_dsi,
1109                                struct mipi_ctrl_cfg *cfg, u32 dphy_no,
1110                                int active_lanes, enum dphy_mode mode)
1111 {
1112         u32 test_code = 0, test_data = 0, val;
1113
1114         /* Set D-PHY in shutdown mode */
1115         /* Assert RSTZ signal */
1116         CLR_DPHY_INIT_CTRL0(kmb_dsi, dphy_no, RESETZ);
1117
1118         /* Assert SHUTDOWNZ signal */
1119         CLR_DPHY_INIT_CTRL0(kmb_dsi, dphy_no, SHUTDOWNZ);
1120         val = kmb_read_mipi(kmb_dsi, DPHY_INIT_CTRL0);
1121
1122         /* Init D-PHY_n
1123          * Pulse testclear signal to make sure the d-phy configuration
1124          * starts from a clean base
1125          */
1126         CLR_DPHY_TEST_CTRL0(kmb_dsi, dphy_no);
1127         ndelay(15);
1128         SET_DPHY_TEST_CTRL0(kmb_dsi, dphy_no);
1129         ndelay(15);
1130         CLR_DPHY_TEST_CTRL0(kmb_dsi, dphy_no);
1131         ndelay(15);
1132
1133         /* Set mastermacro bit - Master or slave mode */
1134         test_code = TEST_CODE_MULTIPLE_PHY_CTRL;
1135
1136         /* DPHY has its own clock lane enabled (master) */
1137         if (mode == MIPI_DPHY_MASTER)
1138                 test_data = 0x01;
1139         else
1140                 test_data = 0x00;
1141
1142         /* Send the test code and data */
1143         test_mode_send(kmb_dsi, dphy_no, test_code, test_data);
1144
1145         /* Set the lane data rate */
1146         set_lane_data_rate(kmb_dsi, dphy_no, cfg);
1147
1148         /* High-Speed Tx Slew Rate Calibration
1149          * BitRate: > 1.5 Gbps && <= 2.5 Gbps: slew rate control OFF
1150          */
1151         if (cfg->lane_rate_mbps > 1500)
1152                 set_slewrate_gt_1500(kmb_dsi, dphy_no);
1153         else if (cfg->lane_rate_mbps > 1000)
1154                 set_slewrate_gt_1000(kmb_dsi, dphy_no);
1155         else
1156                 set_slewrate_lt_1000(kmb_dsi, dphy_no);
1157
1158         /* Set cfgclkfreqrange */
1159         val = (((cfg->cfg_clk_khz / 1000) - 17) * 4) & 0x3f;
1160         SET_DPHY_FREQ_CTRL0_3(kmb_dsi, dphy_no, val);
1161
1162         /* Enable config clk for the corresponding d-phy */
1163         kmb_set_bit_mipi(kmb_dsi, DPHY_CFG_CLK_EN, dphy_no);
1164
1165         /* PLL setup */
1166         if (mode == MIPI_DPHY_MASTER)
1167                 setup_pll(kmb_dsi, dphy_no, cfg);
1168
1169         /* Send NORMAL OPERATION test code */
1170         test_code = 0x0;
1171         test_data = 0x0;
1172         test_mode_send(kmb_dsi, dphy_no, test_code, test_data);
1173
1174         /* Configure BASEDIR for data lanes
1175          * NOTE: basedir only applies to LANE_0 of each D-PHY.
1176          * The other lanes keep their direction based on the D-PHY type,
1177          * either Rx or Tx.
1178          * bits[5:0]  - BaseDir: 1 = Rx
1179          * bits[9:6] - BaseDir: 0 = Tx
1180          */
1181         kmb_write_bits_mipi(kmb_dsi, DPHY_INIT_CTRL2, 0, 9, 0x03f);
1182         ndelay(15);
1183
1184         /* Enable CLOCK LANE
1185          * Clock lane should be enabled regardless of the direction
1186          * set for the D-PHY (Rx/Tx)
1187          */
1188         kmb_set_bit_mipi(kmb_dsi, DPHY_INIT_CTRL2, 12 + dphy_no);
1189
1190         /* Enable DATA LANES */
1191         kmb_write_bits_mipi(kmb_dsi, DPHY_ENABLE, dphy_no * 2, 2,
1192                             ((1 << active_lanes) - 1));
1193
1194         ndelay(15);
1195
1196         /* Take D-PHY out of shutdown mode */
1197         /* Deassert SHUTDOWNZ signal */
1198         SET_DPHY_INIT_CTRL0(kmb_dsi, dphy_no, SHUTDOWNZ);
1199         ndelay(15);
1200
1201         /* Deassert RSTZ signal */
1202         SET_DPHY_INIT_CTRL0(kmb_dsi, dphy_no, RESETZ);
1203 }
1204
1205 static void dphy_wait_fsm(struct kmb_dsi *kmb_dsi, u32 dphy_no,
1206                           enum dphy_tx_fsm fsm_state)
1207 {
1208         enum dphy_tx_fsm val = DPHY_TX_POWERDWN;
1209         int i = 0;
1210         int status = 1;
1211
1212         do {
1213                 test_mode_send(kmb_dsi, dphy_no, TEST_CODE_FSM_CONTROL, 0x80);
1214
1215                 val = GET_TEST_DOUT4_7(kmb_dsi, dphy_no);
1216                 i++;
1217                 if (i > TIMEOUT) {
1218                         status = 0;
1219                         break;
1220                 }
1221         } while (val != fsm_state);
1222
1223         dev_dbg(kmb_dsi->dev, "%s: dphy %d val = %x", __func__, dphy_no, val);
1224         dev_dbg(kmb_dsi->dev, "* DPHY %d WAIT_FSM %s *",
1225                 dphy_no, status ? "SUCCESS" : "FAILED");
1226 }
1227
1228 static void wait_init_done(struct kmb_dsi *kmb_dsi, u32 dphy_no,
1229                            u32 active_lanes)
1230 {
1231         u32 stopstatedata = 0;
1232         u32 data_lanes = (1 << active_lanes) - 1;
1233         int i = 0;
1234         int status = 1;
1235
1236         do {
1237                 stopstatedata = GET_STOPSTATE_DATA(kmb_dsi, dphy_no)
1238                                 & data_lanes;
1239
1240                 /* TODO-need to add a time out and return failure */
1241                 i++;
1242
1243                 if (i > TIMEOUT) {
1244                         status = 0;
1245                         dev_dbg(kmb_dsi->dev,
1246                                 "! WAIT_INIT_DONE: TIMING OUT!(err_stat=%d)",
1247                                 kmb_read_mipi(kmb_dsi, MIPI_DPHY_ERR_STAT6_7));
1248                         break;
1249                 }
1250         } while (stopstatedata != data_lanes);
1251
1252         dev_dbg(kmb_dsi->dev, "* DPHY %d INIT - %s *",
1253                 dphy_no, status ? "SUCCESS" : "FAILED");
1254 }
1255
1256 static void wait_pll_lock(struct kmb_dsi *kmb_dsi, u32 dphy_no)
1257 {
1258         int i = 0;
1259         int status = 1;
1260
1261         do {
1262                 /* TODO-need to add a time out and return failure */
1263                 i++;
1264                 if (i > TIMEOUT) {
1265                         status = 0;
1266                         dev_dbg(kmb_dsi->dev, "%s: timing out", __func__);
1267                         break;
1268                 }
1269         } while (!GET_PLL_LOCK(kmb_dsi, dphy_no));
1270
1271         dev_dbg(kmb_dsi->dev, "* PLL Locked for DPHY %d - %s *",
1272                 dphy_no, status ? "SUCCESS" : "FAILED");
1273 }
1274
1275 static u32 mipi_tx_init_dphy(struct kmb_dsi *kmb_dsi,
1276                              struct mipi_ctrl_cfg *cfg)
1277 {
1278         u32 dphy_no = MIPI_DPHY6;
1279
1280         /* Multiple D-PHYs needed */
1281         if (cfg->active_lanes > MIPI_DPHY_D_LANES) {
1282                 /*
1283                  *Initialization for Tx aggregation mode is done according to
1284                  *a. start init PHY1
1285                  *b. poll for PHY1 FSM state LOCK
1286                  *   b1. reg addr 0x03[3:0] - state_main[3:0] == 5 (LOCK)
1287                  *c. poll for PHY1 calibrations done :
1288                  *   c1. termination calibration lower section: addr 0x22[5]
1289                  *   - rescal_done
1290                  *   c2. slewrate calibration (if data rate < = 1500 Mbps):
1291                  *     addr 0xA7[3:2] - srcal_done, sr_finished
1292                  *d. start init PHY0
1293                  *e. poll for PHY0 stopstate
1294                  *f. poll for PHY1 stopstate
1295                  */
1296                 /* PHY #N+1 ('slave') */
1297
1298                 dphy_init_sequence(kmb_dsi, cfg, dphy_no + 1,
1299                                    (cfg->active_lanes - MIPI_DPHY_D_LANES),
1300                                    MIPI_DPHY_SLAVE);
1301                 dphy_wait_fsm(kmb_dsi, dphy_no + 1, DPHY_TX_LOCK);
1302
1303                 /* PHY #N master */
1304                 dphy_init_sequence(kmb_dsi, cfg, dphy_no, MIPI_DPHY_D_LANES,
1305                                    MIPI_DPHY_MASTER);
1306
1307                 /* Wait for DPHY init to complete */
1308                 wait_init_done(kmb_dsi, dphy_no, MIPI_DPHY_D_LANES);
1309                 wait_init_done(kmb_dsi, dphy_no + 1,
1310                                cfg->active_lanes - MIPI_DPHY_D_LANES);
1311                 wait_pll_lock(kmb_dsi, dphy_no);
1312                 wait_pll_lock(kmb_dsi, dphy_no + 1);
1313                 dphy_wait_fsm(kmb_dsi, dphy_no, DPHY_TX_IDLE);
1314         } else {                /* Single DPHY */
1315                 dphy_init_sequence(kmb_dsi, cfg, dphy_no, cfg->active_lanes,
1316                                    MIPI_DPHY_MASTER);
1317                 dphy_wait_fsm(kmb_dsi, dphy_no, DPHY_TX_IDLE);
1318                 wait_init_done(kmb_dsi, dphy_no, cfg->active_lanes);
1319                 wait_pll_lock(kmb_dsi, dphy_no);
1320         }
1321
1322         return 0;
1323 }
1324
1325 static void connect_lcd_to_mipi(struct kmb_dsi *kmb_dsi)
1326 {
1327         struct regmap *msscam;
1328
1329         msscam = syscon_regmap_lookup_by_compatible("intel,keembay-msscam");
1330         if (IS_ERR(msscam)) {
1331                 dev_dbg(kmb_dsi->dev, "failed to get msscam syscon");
1332                 return;
1333         }
1334
1335         /* DISABLE MIPI->CIF CONNECTION */
1336         regmap_write(msscam, MSS_MIPI_CIF_CFG, 0);
1337
1338         /* ENABLE LCD->MIPI CONNECTION */
1339         regmap_write(msscam, MSS_LCD_MIPI_CFG, 1);
1340         /* DISABLE LCD->CIF LOOPBACK */
1341         regmap_write(msscam, MSS_LOOPBACK_CFG, 1);
1342 }
1343
1344 int kmb_dsi_mode_set(struct kmb_dsi *kmb_dsi, struct drm_display_mode *mode,
1345                      int sys_clk_mhz)
1346 {
1347         u64 data_rate;
1348
1349         kmb_dsi->sys_clk_mhz = sys_clk_mhz;
1350         mipi_tx_init_cfg.active_lanes = MIPI_TX_ACTIVE_LANES;
1351
1352         mipi_tx_frame0_sect_cfg.width_pixels = mode->crtc_hdisplay;
1353         mipi_tx_frame0_sect_cfg.height_lines = mode->crtc_vdisplay;
1354         mipitx_frame0_cfg.vsync_width =
1355                 mode->crtc_vsync_end - mode->crtc_vsync_start;
1356         mipitx_frame0_cfg.v_backporch =
1357                 mode->crtc_vtotal - mode->crtc_vsync_end;
1358         mipitx_frame0_cfg.v_frontporch =
1359                 mode->crtc_vsync_start - mode->crtc_vdisplay;
1360         mipitx_frame0_cfg.hsync_width =
1361                 mode->crtc_hsync_end - mode->crtc_hsync_start;
1362         mipitx_frame0_cfg.h_backporch =
1363                 mode->crtc_htotal - mode->crtc_hsync_end;
1364         mipitx_frame0_cfg.h_frontporch =
1365                 mode->crtc_hsync_start - mode->crtc_hdisplay;
1366
1367         /* Lane rate = (vtotal*htotal*fps*bpp)/4 / 1000000
1368          * to convert to Mbps
1369          */
1370         data_rate = ((((u32)mode->crtc_vtotal * (u32)mode->crtc_htotal) *
1371                         (u32)(drm_mode_vrefresh(mode)) *
1372                         MIPI_TX_BPP) / mipi_tx_init_cfg.active_lanes) / 1000000;
1373
1374         dev_dbg(kmb_dsi->dev, "data_rate=%u active_lanes=%d\n",
1375                 (u32)data_rate, mipi_tx_init_cfg.active_lanes);
1376
1377         /* When late rate < 800, modeset fails with 4 lanes,
1378          * so switch to 2 lanes
1379          */
1380         if (data_rate < 800) {
1381                 mipi_tx_init_cfg.active_lanes = 2;
1382                 mipi_tx_init_cfg.lane_rate_mbps = data_rate * 2;
1383         } else {
1384                 mipi_tx_init_cfg.lane_rate_mbps = data_rate;
1385         }
1386
1387         kmb_write_mipi(kmb_dsi, DPHY_ENABLE, 0);
1388         kmb_write_mipi(kmb_dsi, DPHY_INIT_CTRL0, 0);
1389         kmb_write_mipi(kmb_dsi, DPHY_INIT_CTRL1, 0);
1390         kmb_write_mipi(kmb_dsi, DPHY_INIT_CTRL2, 0);
1391
1392         /* Initialize mipi controller */
1393         mipi_tx_init_cntrl(kmb_dsi, &mipi_tx_init_cfg);
1394
1395         /* Dphy initialization */
1396         mipi_tx_init_dphy(kmb_dsi, &mipi_tx_init_cfg);
1397
1398         connect_lcd_to_mipi(kmb_dsi);
1399         dev_info(kmb_dsi->dev, "mipi hw initialized");
1400
1401         return 0;
1402 }
1403
1404 struct kmb_dsi *kmb_dsi_init(struct platform_device *pdev)
1405 {
1406         struct kmb_dsi *kmb_dsi;
1407         struct device *dev = get_device(&pdev->dev);
1408
1409         kmb_dsi = devm_kzalloc(dev, sizeof(*kmb_dsi), GFP_KERNEL);
1410         if (!kmb_dsi) {
1411                 dev_err(dev, "failed to allocate kmb_dsi\n");
1412                 return ERR_PTR(-ENOMEM);
1413         }
1414
1415         kmb_dsi->host = dsi_host;
1416         kmb_dsi->host->ops = &kmb_dsi_host_ops;
1417
1418         dsi_device->host = kmb_dsi->host;
1419         kmb_dsi->device = dsi_device;
1420
1421         return kmb_dsi;
1422 }
1423
1424 int kmb_dsi_encoder_init(struct drm_device *dev, struct kmb_dsi *kmb_dsi)
1425 {
1426         struct drm_encoder *encoder;
1427         struct drm_connector *connector;
1428         int ret = 0;
1429
1430         encoder = &kmb_dsi->base;
1431         encoder->possible_crtcs = 1;
1432         encoder->possible_clones = 0;
1433
1434         ret = drm_simple_encoder_init(dev, encoder, DRM_MODE_ENCODER_DSI);
1435         if (ret) {
1436                 dev_err(kmb_dsi->dev, "Failed to init encoder %d\n", ret);
1437                 return ret;
1438         }
1439
1440         /* Link drm_bridge to encoder */
1441         ret = drm_bridge_attach(encoder, adv_bridge, NULL,
1442                                 DRM_BRIDGE_ATTACH_NO_CONNECTOR);
1443         if (ret) {
1444                 drm_encoder_cleanup(encoder);
1445                 return ret;
1446         }
1447         drm_info(dev, "Bridge attached : SUCCESS");
1448         connector = drm_bridge_connector_init(dev, encoder);
1449         if (IS_ERR(connector)) {
1450                 DRM_ERROR("Unable to create bridge connector");
1451                 drm_encoder_cleanup(encoder);
1452                 return PTR_ERR(connector);
1453         }
1454         drm_connector_attach_encoder(connector, encoder);
1455         return 0;
1456 }
1457
1458 int kmb_dsi_map_mmio(struct kmb_dsi *kmb_dsi)
1459 {
1460         struct resource *res;
1461         struct device *dev = kmb_dsi->dev;
1462
1463         res = platform_get_resource_byname(kmb_dsi->pdev, IORESOURCE_MEM,
1464                                            "mipi");
1465         if (!res) {
1466                 dev_err(dev, "failed to get resource for mipi");
1467                 return -ENOMEM;
1468         }
1469         kmb_dsi->mipi_mmio = devm_ioremap_resource(dev, res);
1470         if (IS_ERR(kmb_dsi->mipi_mmio)) {
1471                 dev_err(dev, "failed to ioremap mipi registers");
1472                 return PTR_ERR(kmb_dsi->mipi_mmio);
1473         }
1474         return 0;
1475 }
1476
1477 static int kmb_dsi_clk_enable(struct kmb_dsi *kmb_dsi)
1478 {
1479         int ret;
1480         struct device *dev = kmb_dsi->dev;
1481
1482         ret = clk_prepare_enable(kmb_dsi->clk_mipi);
1483         if (ret) {
1484                 dev_err(dev, "Failed to enable MIPI clock: %d\n", ret);
1485                 return ret;
1486         }
1487
1488         ret = clk_prepare_enable(kmb_dsi->clk_mipi_ecfg);
1489         if (ret) {
1490                 dev_err(dev, "Failed to enable MIPI_ECFG clock: %d\n", ret);
1491                 return ret;
1492         }
1493
1494         ret = clk_prepare_enable(kmb_dsi->clk_mipi_cfg);
1495         if (ret) {
1496                 dev_err(dev, "Failed to enable MIPI_CFG clock: %d\n", ret);
1497                 return ret;
1498         }
1499
1500         dev_info(dev, "SUCCESS : enabled MIPI clocks\n");
1501         return 0;
1502 }
1503
1504 int kmb_dsi_clk_init(struct kmb_dsi *kmb_dsi)
1505 {
1506         struct device *dev = kmb_dsi->dev;
1507         unsigned long clk;
1508
1509         kmb_dsi->clk_mipi = devm_clk_get(dev, "clk_mipi");
1510         if (IS_ERR(kmb_dsi->clk_mipi)) {
1511                 dev_err(dev, "devm_clk_get() failed clk_mipi\n");
1512                 return PTR_ERR(kmb_dsi->clk_mipi);
1513         }
1514
1515         kmb_dsi->clk_mipi_ecfg = devm_clk_get(dev, "clk_mipi_ecfg");
1516         if (IS_ERR(kmb_dsi->clk_mipi_ecfg)) {
1517                 dev_err(dev, "devm_clk_get() failed clk_mipi_ecfg\n");
1518                 return PTR_ERR(kmb_dsi->clk_mipi_ecfg);
1519         }
1520
1521         kmb_dsi->clk_mipi_cfg = devm_clk_get(dev, "clk_mipi_cfg");
1522         if (IS_ERR(kmb_dsi->clk_mipi_cfg)) {
1523                 dev_err(dev, "devm_clk_get() failed clk_mipi_cfg\n");
1524                 return PTR_ERR(kmb_dsi->clk_mipi_cfg);
1525         }
1526         /* Set MIPI clock to 24 Mhz */
1527         clk_set_rate(kmb_dsi->clk_mipi, KMB_MIPI_DEFAULT_CLK);
1528         if (clk_get_rate(kmb_dsi->clk_mipi) != KMB_MIPI_DEFAULT_CLK) {
1529                 dev_err(dev, "failed to set to clk_mipi to %d\n",
1530                         KMB_MIPI_DEFAULT_CLK);
1531                 return -1;
1532         }
1533         dev_dbg(dev, "clk_mipi = %ld\n", clk_get_rate(kmb_dsi->clk_mipi));
1534
1535         clk = clk_get_rate(kmb_dsi->clk_mipi_ecfg);
1536         if (clk != KMB_MIPI_DEFAULT_CFG_CLK) {
1537                 /* Set MIPI_ECFG clock to 24 Mhz */
1538                 clk_set_rate(kmb_dsi->clk_mipi_ecfg, KMB_MIPI_DEFAULT_CFG_CLK);
1539                 clk = clk_get_rate(kmb_dsi->clk_mipi_ecfg);
1540                 if (clk != KMB_MIPI_DEFAULT_CFG_CLK) {
1541                         dev_err(dev, "failed to set to clk_mipi_ecfg to %d\n",
1542                                 KMB_MIPI_DEFAULT_CFG_CLK);
1543                         return -1;
1544                 }
1545         }
1546
1547         clk = clk_get_rate(kmb_dsi->clk_mipi_cfg);
1548         if (clk != KMB_MIPI_DEFAULT_CFG_CLK) {
1549                 /* Set MIPI_CFG clock to 24 Mhz */
1550                 clk_set_rate(kmb_dsi->clk_mipi_cfg, 24000000);
1551                 clk = clk_get_rate(kmb_dsi->clk_mipi_cfg);
1552                 if (clk != KMB_MIPI_DEFAULT_CFG_CLK) {
1553                         dev_err(dev, "failed to set clk_mipi_cfg to %d\n",
1554                                 KMB_MIPI_DEFAULT_CFG_CLK);
1555                         return -1;
1556                 }
1557         }
1558
1559         return kmb_dsi_clk_enable(kmb_dsi);
1560 }