drivers/gpu/drm/mgag200/mgag200_mode.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2 /*
   3  * Copyright 2010 Matt Turner.
   4  * Copyright 2012 Red Hat
   5  *
   6  * Authors: Matthew Garrett
   7  *          Matt Turner
   8  *          Dave Airlie
   9  */
  10
  11 #include <linux/delay.h>
  12
  13 #include <drm/drmP.h>
  14 #include <drm/drm_crtc_helper.h>
  15 #include <drm/drm_plane_helper.h>
  16 #include <drm/drm_probe_helper.h>
  17
  18 #include "mgag200_drv.h"
  19
  20 #define MGAG200_LUT_SIZE 256
  21
  22 /*
  23  * This file contains setup code for the CRTC.
  24  */
  25
  26 static void mga_crtc_load_lut(struct drm_crtc *crtc)
  27 {
  28         struct drm_device *dev = crtc->dev;
  29         struct mga_device *mdev = dev->dev_private;
  30         struct drm_framebuffer *fb = crtc->primary->fb;
  31         u16 *r_ptr, *g_ptr, *b_ptr;
  32         int i;
  33
  34         if (!crtc->enabled)
  35                 return;
  36
  37         r_ptr = crtc->gamma_store;
  38         g_ptr = r_ptr + crtc->gamma_size;
  39         b_ptr = g_ptr + crtc->gamma_size;
  40
  41         WREG8(DAC_INDEX + MGA1064_INDEX, 0);
  42
  43         if (fb && fb->format->cpp[0] * 8 == 16) {
  44                 int inc = (fb->format->depth == 15) ? 8 : 4;
  45                 u8 r, b;
  46                 for (i = 0; i < MGAG200_LUT_SIZE; i += inc) {
  47                         if (fb->format->depth == 16) {
  48                                 if (i > (MGAG200_LUT_SIZE >> 1)) {
  49                                         r = b = 0;
  50                                 } else {
  51                                         r = *r_ptr++ >> 8;
  52                                         b = *b_ptr++ >> 8;
  53                                         r_ptr++;
  54                                         b_ptr++;
  55                                 }
  56                         } else {
  57                                 r = *r_ptr++ >> 8;
  58                                 b = *b_ptr++ >> 8;
  59                         }
  60                         /* VGA registers */
  61                         WREG8(DAC_INDEX + MGA1064_COL_PAL, r);
  62                         WREG8(DAC_INDEX + MGA1064_COL_PAL, *g_ptr++ >> 8);
  63                         WREG8(DAC_INDEX + MGA1064_COL_PAL, b);
  64                 }
  65                 return;
  66         }
  67         for (i = 0; i < MGAG200_LUT_SIZE; i++) {
  68                 /* VGA registers */
  69                 WREG8(DAC_INDEX + MGA1064_COL_PAL, *r_ptr++ >> 8);
  70                 WREG8(DAC_INDEX + MGA1064_COL_PAL, *g_ptr++ >> 8);
  71                 WREG8(DAC_INDEX + MGA1064_COL_PAL, *b_ptr++ >> 8);
  72         }
  73 }
  74
  75 static inline void mga_wait_vsync(struct mga_device *mdev)
  76 {
  77         unsigned long timeout = jiffies + HZ/10;
  78         unsigned int status = 0;
  79
  80         do {
  81                 status = RREG32(MGAREG_Status);
  82         } while ((status & 0x08) && time_before(jiffies, timeout));
  83         timeout = jiffies + HZ/10;
  84         status = 0;
  85         do {
  86                 status = RREG32(MGAREG_Status);
  87         } while (!(status & 0x08) && time_before(jiffies, timeout));
  88 }
  89
  90 static inline void mga_wait_busy(struct mga_device *mdev)
  91 {
  92         unsigned long timeout = jiffies + HZ;
  93         unsigned int status = 0;
  94         do {
  95                 status = RREG8(MGAREG_Status + 2);
  96         } while ((status & 0x01) && time_before(jiffies, timeout));
  97 }
  98
  99 #define P_ARRAY_SIZE 9
 100
 101 static int mga_g200se_set_plls(struct mga_device *mdev, long clock)
 102 {
 103         unsigned int vcomax, vcomin, pllreffreq;
 104         unsigned int delta, tmpdelta, permitteddelta;
 105         unsigned int testp, testm, testn;
 106         unsigned int p, m, n;
 107         unsigned int computed;
 108         unsigned int pvalues_e4[P_ARRAY_SIZE] = {16, 14, 12, 10, 8, 6, 4, 2, 1};
 109         unsigned int fvv;
 110         unsigned int i;
 111
 112         if (mdev->unique_rev_id <= 0x03) {
 113
 114                 m = n = p = 0;
 115                 vcomax = 320000;
 116                 vcomin = 160000;
 117                 pllreffreq = 25000;
 118
 119                 delta = 0xffffffff;
 120                 permitteddelta = clock * 5 / 1000;
 121
 122                 for (testp = 8; testp > 0; testp /= 2) {
 123                         if (clock * testp > vcomax)
 124                                 continue;
 125                         if (clock * testp < vcomin)
 126                                 continue;
 127
 128                         for (testn = 17; testn < 256; testn++) {
 129                                 for (testm = 1; testm < 32; testm++) {
 130                                         computed = (pllreffreq * testn) /
 131                                                 (testm * testp);
 132                                         if (computed > clock)
 133                                                 tmpdelta = computed - clock;
 134                                         else
 135                                                 tmpdelta = clock - computed;
 136                                         if (tmpdelta < delta) {
 137                                                 delta = tmpdelta;
 138                                                 m = testm - 1;
 139                                                 n = testn - 1;
 140                                                 p = testp - 1;
 141                                         }
 142                                 }
 143                         }
 144                 }
 145         } else {
 146
 147
 148                 m = n = p = 0;
 149                 vcomax        = 1600000;
 150                 vcomin        = 800000;
 151                 pllreffreq    = 25000;
 152
 153                 if (clock < 25000)
 154                         clock = 25000;
 155
 156                 clock = clock * 2;
 157
 158                 delta = 0xFFFFFFFF;
 159                 /* Permited delta is 0.5% as VESA Specification */
 160                 permitteddelta = clock * 5 / 1000;
 161
 162                 for (i = 0 ; i < P_ARRAY_SIZE ; i++) {
 163                         testp = pvalues_e4[i];
 164
 165                         if ((clock * testp) > vcomax)
 166                                 continue;
 167                         if ((clock * testp) < vcomin)
 168                                 continue;
 169
 170                         for (testn = 50; testn <= 256; testn++) {
 171                                 for (testm = 1; testm <= 32; testm++) {
 172                                         computed = (pllreffreq * testn) /
 173                                                 (testm * testp);
 174                                         if (computed > clock)
 175                                                 tmpdelta = computed - clock;
 176                                         else
 177                                                 tmpdelta = clock - computed;
 178
 179                                         if (tmpdelta < delta) {
 180                                                 delta = tmpdelta;
 181                                                 m = testm - 1;
 182                                                 n = testn - 1;
 183                                                 p = testp - 1;
 184                                         }
 185                                 }
 186                         }
 187                 }
 188
 189                 fvv = pllreffreq * (n + 1) / (m + 1);
 190                 fvv = (fvv - 800000) / 50000;
 191
 192                 if (fvv > 15)
 193                         fvv = 15;
 194
 195                 p |= (fvv << 4);
 196                 m |= 0x80;
 197
 198                 clock = clock / 2;
 199         }
 200
 201         if (delta > permitteddelta) {
 202                 pr_warn("PLL delta too large\n");
 203                 return 1;
 204         }
 205
 206         WREG_DAC(MGA1064_PIX_PLLC_M, m);
 207         WREG_DAC(MGA1064_PIX_PLLC_N, n);
 208         WREG_DAC(MGA1064_PIX_PLLC_P, p);
 209
 210         if (mdev->unique_rev_id >= 0x04) {
 211                 WREG_DAC(0x1a, 0x09);
 212                 msleep(20);
 213                 WREG_DAC(0x1a, 0x01);
 214
 215         }
 216
 217         return 0;
 218 }
 219
 220 static int mga_g200wb_set_plls(struct mga_device *mdev, long clock)
 221 {
 222         unsigned int vcomax, vcomin, pllreffreq;
 223         unsigned int delta, tmpdelta;
 224         unsigned int testp, testm, testn, testp2;
 225         unsigned int p, m, n;
 226         unsigned int computed;
 227         int i, j, tmpcount, vcount;
 228         bool pll_locked = false;
 229         u8 tmp;
 230
 231         m = n = p = 0;
 232
 233         delta = 0xffffffff;
 234
 235         if (mdev->type == G200_EW3) {
 236
 237                 vcomax = 800000;
 238                 vcomin = 400000;
 239                 pllreffreq = 25000;
 240
 241                 for (testp = 1; testp < 8; testp++) {
 242                         for (testp2 = 1; testp2 < 8; testp2++) {
 243                                 if (testp < testp2)
 244                                         continue;
 245                                 if ((clock * testp * testp2) > vcomax)
 246                                         continue;
 247                                 if ((clock * testp * testp2) < vcomin)
 248                                         continue;
 249                                 for (testm = 1; testm < 26; testm++) {
 250                                         for (testn = 32; testn < 2048 ; testn++) {
 251                                                 computed = (pllreffreq * testn) /
 252                                                         (testm * testp * testp2);
 253                                                 if (computed > clock)
 254                                                         tmpdelta = computed - clock;
 255                                                 else
 256                                                         tmpdelta = clock - computed;
 257                                                 if (tmpdelta < delta) {
 258                                                         delta = tmpdelta;
 259                                                         m = ((testn & 0x100) >> 1) |
 260                                                                 (testm);
 261                                                         n = (testn & 0xFF);
 262                                                         p = ((testn & 0x600) >> 3) |
 263                                                                 (testp2 << 3) |
 264                                                                 (testp);
 265                                                 }
 266                                         }
 267                                 }
 268                         }
 269                 }
 270         } else {
 271
 272                 vcomax = 550000;
 273                 vcomin = 150000;
 274                 pllreffreq = 48000;
 275
 276                 for (testp = 1; testp < 9; testp++) {
 277                         if (clock * testp > vcomax)
 278                                 continue;
 279                         if (clock * testp < vcomin)
 280                                 continue;
 281
 282                         for (testm = 1; testm < 17; testm++) {
 283                                 for (testn = 1; testn < 151; testn++) {
 284                                         computed = (pllreffreq * testn) /
 285                                                 (testm * testp);
 286                                         if (computed > clock)
 287                                                 tmpdelta = computed - clock;
 288                                         else
 289                                                 tmpdelta = clock - computed;
 290                                         if (tmpdelta < delta) {
 291                                                 delta = tmpdelta;
 292                                                 n = testn - 1;
 293                                                 m = (testm - 1) |
 294                                                         ((n >> 1) & 0x80);
 295                                                 p = testp - 1;
 296                                         }
 297                                 }
 298                         }
 299                 }
 300         }
 301
 302         for (i = 0; i <= 32 && pll_locked == false; i++) {
 303                 if (i > 0) {
 304                         WREG8(MGAREG_CRTC_INDEX, 0x1e);
 305                         tmp = RREG8(MGAREG_CRTC_DATA);
 306                         if (tmp < 0xff)
 307                                 WREG8(MGAREG_CRTC_DATA, tmp+1);
 308                 }
 309
 310                 /* set pixclkdis to 1 */
 311                 WREG8(DAC_INDEX, MGA1064_PIX_CLK_CTL);
 312                 tmp = RREG8(DAC_DATA);
 313                 tmp |= MGA1064_PIX_CLK_CTL_CLK_DIS;
 314                 WREG8(DAC_DATA, tmp);
 315
 316                 WREG8(DAC_INDEX, MGA1064_REMHEADCTL);
 317                 tmp = RREG8(DAC_DATA);
 318                 tmp |= MGA1064_REMHEADCTL_CLKDIS;
 319                 WREG8(DAC_DATA, tmp);
 320
 321                 /* select PLL Set C */
 322                 tmp = RREG8(MGAREG_MEM_MISC_READ);
 323                 tmp |= 0x3 << 2;
 324                 WREG8(MGAREG_MEM_MISC_WRITE, tmp);
 325
 326                 WREG8(DAC_INDEX, MGA1064_PIX_CLK_CTL);
 327                 tmp = RREG8(DAC_DATA);
 328                 tmp |= MGA1064_PIX_CLK_CTL_CLK_POW_DOWN | 0x80;
 329                 WREG8(DAC_DATA, tmp);
 330
 331                 udelay(500);
 332
 333                 /* reset the PLL */
 334                 WREG8(DAC_INDEX, MGA1064_VREF_CTL);
 335                 tmp = RREG8(DAC_DATA);
 336                 tmp &= ~0x04;
 337                 WREG8(DAC_DATA, tmp);
 338
 339                 udelay(50);
 340
 341                 /* program pixel pll register */
 342                 WREG_DAC(MGA1064_WB_PIX_PLLC_N, n);
 343                 WREG_DAC(MGA1064_WB_PIX_PLLC_M, m);
 344                 WREG_DAC(MGA1064_WB_PIX_PLLC_P, p);
 345
 346                 udelay(50);
 347
 348                 /* turn pll on */
 349                 WREG8(DAC_INDEX, MGA1064_VREF_CTL);
 350                 tmp = RREG8(DAC_DATA);
 351                 tmp |= 0x04;
 352                 WREG_DAC(MGA1064_VREF_CTL, tmp);
 353
 354                 udelay(500);
 355
 356                 /* select the pixel pll */
 357                 WREG8(DAC_INDEX, MGA1064_PIX_CLK_CTL);
 358                 tmp = RREG8(DAC_DATA);
 359                 tmp &= ~MGA1064_PIX_CLK_CTL_SEL_MSK;
 360                 tmp |= MGA1064_PIX_CLK_CTL_SEL_PLL;
 361                 WREG8(DAC_DATA, tmp);
 362
 363                 WREG8(DAC_INDEX, MGA1064_REMHEADCTL);
 364                 tmp = RREG8(DAC_DATA);
 365                 tmp &= ~MGA1064_REMHEADCTL_CLKSL_MSK;
 366                 tmp |= MGA1064_REMHEADCTL_CLKSL_PLL;
 367                 WREG8(DAC_DATA, tmp);
 368
 369                 /* reset dotclock rate bit */
 370                 WREG8(MGAREG_SEQ_INDEX, 1);
 371                 tmp = RREG8(MGAREG_SEQ_DATA);
 372                 tmp &= ~0x8;
 373                 WREG8(MGAREG_SEQ_DATA, tmp);
 374
 375                 WREG8(DAC_INDEX, MGA1064_PIX_CLK_CTL);
 376                 tmp = RREG8(DAC_DATA);
 377                 tmp &= ~MGA1064_PIX_CLK_CTL_CLK_DIS;
 378                 WREG8(DAC_DATA, tmp);
 379
 380                 vcount = RREG8(MGAREG_VCOUNT);
 381
 382                 for (j = 0; j < 30 && pll_locked == false; j++) {
 383                         tmpcount = RREG8(MGAREG_VCOUNT);
 384                         if (tmpcount < vcount)
 385                                 vcount = 0;
 386                         if ((tmpcount - vcount) > 2)
 387                                 pll_locked = true;
 388                         else
 389                                 udelay(5);
 390                 }
 391         }
 392         WREG8(DAC_INDEX, MGA1064_REMHEADCTL);
 393         tmp = RREG8(DAC_DATA);
 394         tmp &= ~MGA1064_REMHEADCTL_CLKDIS;
 395         WREG_DAC(MGA1064_REMHEADCTL, tmp);
 396         return 0;
 397 }
 398
 399 static int mga_g200ev_set_plls(struct mga_device *mdev, long clock)
 400 {
 401         unsigned int vcomax, vcomin, pllreffreq;
 402         unsigned int delta, tmpdelta;
 403         unsigned int testp, testm, testn;
 404         unsigned int p, m, n;
 405         unsigned int computed;
 406         u8 tmp;
 407
 408         m = n = p = 0;
 409         vcomax = 550000;
 410         vcomin = 150000;
 411         pllreffreq = 50000;
 412
 413         delta = 0xffffffff;
 414
 415         for (testp = 16; testp > 0; testp--) {
 416                 if (clock * testp > vcomax)
 417                         continue;
 418                 if (clock * testp < vcomin)
 419                         continue;
 420
 421                 for (testn = 1; testn < 257; testn++) {
 422                         for (testm = 1; testm < 17; testm++) {
 423                                 computed = (pllreffreq * testn) /
 424                                         (testm * testp);
 425                                 if (computed > clock)
 426                                         tmpdelta = computed - clock;
 427                                 else
 428                                         tmpdelta = clock - computed;
 429                                 if (tmpdelta < delta) {
 430                                         delta = tmpdelta;
 431                                         n = testn - 1;
 432                                         m = testm - 1;
 433                                         p = testp - 1;
 434                                 }
 435                         }
 436                 }
 437         }
 438
 439         WREG8(DAC_INDEX, MGA1064_PIX_CLK_CTL);
 440         tmp = RREG8(DAC_DATA);
 441         tmp |= MGA1064_PIX_CLK_CTL_CLK_DIS;
 442         WREG8(DAC_DATA, tmp);
 443
 444         tmp = RREG8(MGAREG_MEM_MISC_READ);
 445         tmp |= 0x3 << 2;
 446         WREG8(MGAREG_MEM_MISC_WRITE, tmp);
 447
 448         WREG8(DAC_INDEX, MGA1064_PIX_PLL_STAT);
 449         tmp = RREG8(DAC_DATA);
 450         WREG8(DAC_DATA, tmp & ~0x40);
 451
 452         WREG8(DAC_INDEX, MGA1064_PIX_CLK_CTL);
 453         tmp = RREG8(DAC_DATA);
 454         tmp |= MGA1064_PIX_CLK_CTL_CLK_POW_DOWN;
 455         WREG8(DAC_DATA, tmp);
 456
 457         WREG_DAC(MGA1064_EV_PIX_PLLC_M, m);
 458         WREG_DAC(MGA1064_EV_PIX_PLLC_N, n);
 459         WREG_DAC(MGA1064_EV_PIX_PLLC_P, p);
 460
 461         udelay(50);
 462
 463         WREG8(DAC_INDEX, MGA1064_PIX_CLK_CTL);
 464         tmp = RREG8(DAC_DATA);
 465         tmp &= ~MGA1064_PIX_CLK_CTL_CLK_POW_DOWN;
 466         WREG8(DAC_DATA, tmp);
 467
 468         udelay(500);
 469
 470         WREG8(DAC_INDEX, MGA1064_PIX_CLK_CTL);
 471         tmp = RREG8(DAC_DATA);
 472         tmp &= ~MGA1064_PIX_CLK_CTL_SEL_MSK;
 473         tmp |= MGA1064_PIX_CLK_CTL_SEL_PLL;
 474         WREG8(DAC_DATA, tmp);
 475
 476         WREG8(DAC_INDEX, MGA1064_PIX_PLL_STAT);
 477         tmp = RREG8(DAC_DATA);
 478         WREG8(DAC_DATA, tmp | 0x40);
 479
 480         tmp = RREG8(MGAREG_MEM_MISC_READ);
 481         tmp |= (0x3 << 2);
 482         WREG8(MGAREG_MEM_MISC_WRITE, tmp);
 483
 484         WREG8(DAC_INDEX, MGA1064_PIX_CLK_CTL);
 485         tmp = RREG8(DAC_DATA);
 486         tmp &= ~MGA1064_PIX_CLK_CTL_CLK_DIS;
 487         WREG8(DAC_DATA, tmp);
 488
 489         return 0;
 490 }
 491
 492 static int mga_g200eh_set_plls(struct mga_device *mdev, long clock)
 493 {
 494         unsigned int vcomax, vcomin, pllreffreq;
 495         unsigned int delta, tmpdelta;
 496         unsigned int testp, testm, testn;
 497         unsigned int p, m, n;
 498         unsigned int computed;
 499         int i, j, tmpcount, vcount;
 500         u8 tmp;
 501         bool pll_locked = false;
 502
 503         m = n = p = 0;
 504
 505         if (mdev->type == G200_EH3) {
 506                 vcomax = 3000000;
 507                 vcomin = 1500000;
 508                 pllreffreq = 25000;
 509
 510                 delta = 0xffffffff;
 511
 512                 testp = 0;
 513
 514                 for (testm = 150; testm >= 6; testm--) {
 515                         if (clock * testm > vcomax)
 516                                 continue;
 517                         if (clock * testm < vcomin)
 518                                 continue;
 519                         for (testn = 120; testn >= 60; testn--) {
 520                                 computed = (pllreffreq * testn) / testm;
 521                                 if (computed > clock)
 522                                         tmpdelta = computed - clock;
 523                                 else
 524                                         tmpdelta = clock - computed;
 525                                 if (tmpdelta < delta) {
 526                                         delta = tmpdelta;
 527                                         n = testn;
 528                                         m = testm;
 529                                         p = testp;
 530                                 }
 531                                 if (delta == 0)
 532                                         break;
 533                         }
 534                         if (delta == 0)
 535                                 break;
 536                 }
 537         } else {
 538
 539                 vcomax = 800000;
 540                 vcomin = 400000;
 541                 pllreffreq = 33333;
 542
 543                 delta = 0xffffffff;
 544
 545                 for (testp = 16; testp > 0; testp >>= 1) {
 546                         if (clock * testp > vcomax)
 547                                 continue;
 548                         if (clock * testp < vcomin)
 549                                 continue;
 550
 551                         for (testm = 1; testm < 33; testm++) {
 552                                 for (testn = 17; testn < 257; testn++) {
 553                                         computed = (pllreffreq * testn) /
 554                                                 (testm * testp);
 555                                         if (computed > clock)
 556                                                 tmpdelta = computed - clock;
 557                                         else
 558                                                 tmpdelta = clock - computed;
 559                                         if (tmpdelta < delta) {
 560                                                 delta = tmpdelta;
 561                                                 n = testn - 1;
 562                                                 m = (testm - 1);
 563                                                 p = testp - 1;
 564                                         }
 565                                         if ((clock * testp) >= 600000)
 566                                                 p |= 0x80;
 567                                 }
 568                         }
 569                 }
 570         }
 571         for (i = 0; i <= 32 && pll_locked == false; i++) {
 572                 WREG8(DAC_INDEX, MGA1064_PIX_CLK_CTL);
 573                 tmp = RREG8(DAC_DATA);
 574                 tmp |= MGA1064_PIX_CLK_CTL_CLK_DIS;
 575                 WREG8(DAC_DATA, tmp);
 576
 577                 tmp = RREG8(MGAREG_MEM_MISC_READ);
 578                 tmp |= 0x3 << 2;
 579                 WREG8(MGAREG_MEM_MISC_WRITE, tmp);
 580
 581                 WREG8(DAC_INDEX, MGA1064_PIX_CLK_CTL);
 582                 tmp = RREG8(DAC_DATA);
 583                 tmp |= MGA1064_PIX_CLK_CTL_CLK_POW_DOWN;
 584                 WREG8(DAC_DATA, tmp);
 585
 586                 udelay(500);
 587
 588                 WREG_DAC(MGA1064_EH_PIX_PLLC_M, m);
 589                 WREG_DAC(MGA1064_EH_PIX_PLLC_N, n);
 590                 WREG_DAC(MGA1064_EH_PIX_PLLC_P, p);
 591
 592                 udelay(500);
 593
 594                 WREG8(DAC_INDEX, MGA1064_PIX_CLK_CTL);
 595                 tmp = RREG8(DAC_DATA);
 596                 tmp &= ~MGA1064_PIX_CLK_CTL_SEL_MSK;
 597                 tmp |= MGA1064_PIX_CLK_CTL_SEL_PLL;
 598                 WREG8(DAC_DATA, tmp);
 599
 600                 WREG8(DAC_INDEX, MGA1064_PIX_CLK_CTL);
 601                 tmp = RREG8(DAC_DATA);
 602                 tmp &= ~MGA1064_PIX_CLK_CTL_CLK_DIS;
 603                 tmp &= ~MGA1064_PIX_CLK_CTL_CLK_POW_DOWN;
 604                 WREG8(DAC_DATA, tmp);
 605
 606                 vcount = RREG8(MGAREG_VCOUNT);
 607
 608                 for (j = 0; j < 30 && pll_locked == false; j++) {
 609                         tmpcount = RREG8(MGAREG_VCOUNT);
 610                         if (tmpcount < vcount)
 611                                 vcount = 0;
 612                         if ((tmpcount - vcount) > 2)
 613                                 pll_locked = true;
 614                         else
 615                                 udelay(5);
 616                 }
 617         }
 618
 619         return 0;
 620 }
 621
 622 static int mga_g200er_set_plls(struct mga_device *mdev, long clock)
 623 {
 624         unsigned int vcomax, vcomin, pllreffreq;
 625         unsigned int delta, tmpdelta;
 626         int testr, testn, testm, testo;
 627         unsigned int p, m, n;
 628         unsigned int computed, vco;
 629         int tmp;
 630         const unsigned int m_div_val[] = { 1, 2, 4, 8 };
 631
 632         m = n = p = 0;
 633         vcomax = 1488000;
 634         vcomin = 1056000;
 635         pllreffreq = 48000;
 636
 637         delta = 0xffffffff;
 638
 639         for (testr = 0; testr < 4; testr++) {
 640                 if (delta == 0)
 641                         break;
 642                 for (testn = 5; testn < 129; testn++) {
 643                         if (delta == 0)
 644                                 break;
 645                         for (testm = 3; testm >= 0; testm--) {
 646                                 if (delta == 0)
 647                                         break;
 648                                 for (testo = 5; testo < 33; testo++) {
 649                                         vco = pllreffreq * (testn + 1) /
 650                                                 (testr + 1);
 651                                         if (vco < vcomin)
 652                                                 continue;
 653                                         if (vco > vcomax)
 654                                                 continue;
 655                                         computed = vco / (m_div_val[testm] * (testo + 1));
 656                                         if (computed > clock)
 657                                                 tmpdelta = computed - clock;
 658                                         else
 659                                                 tmpdelta = clock - computed;
 660                                         if (tmpdelta < delta) {
 661                                                 delta = tmpdelta;
 662                                                 m = testm | (testo << 3);
 663                                                 n = testn;
 664                                                 p = testr | (testr << 3);
 665                                         }
 666                                 }
 667                         }
 668                 }
 669         }
 670
 671         WREG8(DAC_INDEX, MGA1064_PIX_CLK_CTL);
 672         tmp = RREG8(DAC_DATA);
 673         tmp |= MGA1064_PIX_CLK_CTL_CLK_DIS;
 674         WREG8(DAC_DATA, tmp);
 675
 676         WREG8(DAC_INDEX, MGA1064_REMHEADCTL);
 677         tmp = RREG8(DAC_DATA);
 678         tmp |= MGA1064_REMHEADCTL_CLKDIS;
 679         WREG8(DAC_DATA, tmp);
 680
 681         tmp = RREG8(MGAREG_MEM_MISC_READ);
 682         tmp |= (0x3<<2) | 0xc0;
 683         WREG8(MGAREG_MEM_MISC_WRITE, tmp);
 684
 685         WREG8(DAC_INDEX, MGA1064_PIX_CLK_CTL);
 686         tmp = RREG8(DAC_DATA);
 687         tmp &= ~MGA1064_PIX_CLK_CTL_CLK_DIS;
 688         tmp |= MGA1064_PIX_CLK_CTL_CLK_POW_DOWN;
 689         WREG8(DAC_DATA, tmp);
 690
 691         udelay(500);
 692
 693         WREG_DAC(MGA1064_ER_PIX_PLLC_N, n);
 694         WREG_DAC(MGA1064_ER_PIX_PLLC_M, m);
 695         WREG_DAC(MGA1064_ER_PIX_PLLC_P, p);
 696
 697         udelay(50);
 698
 699         return 0;
 700 }
 701
 702 static int mga_crtc_set_plls(struct mga_device *mdev, long clock)
 703 {
 704         switch(mdev->type) {
 705         case G200_SE_A:
 706         case G200_SE_B:
 707                 return mga_g200se_set_plls(mdev, clock);
 708                 break;
 709         case G200_WB:
 710         case G200_EW3:
 711                 return mga_g200wb_set_plls(mdev, clock);
 712                 break;
 713         case G200_EV:
 714                 return mga_g200ev_set_plls(mdev, clock);
 715                 break;
 716         case G200_EH:
 717         case G200_EH3:
 718                 return mga_g200eh_set_plls(mdev, clock);
 719                 break;
 720         case G200_ER:
 721                 return mga_g200er_set_plls(mdev, clock);
 722                 break;
 723         }
 724         return 0;
 725 }
 726
 727 static void mga_g200wb_prepare(struct drm_crtc *crtc)
 728 {
 729         struct mga_device *mdev = crtc->dev->dev_private;
 730         u8 tmp;
 731         int iter_max;
 732
 733         /* 1- The first step is to warn the BMC of an upcoming mode change.
 734          * We are putting the misc<0> to output.*/
 735
 736         WREG8(DAC_INDEX, MGA1064_GEN_IO_CTL);
 737         tmp = RREG8(DAC_DATA);
 738         tmp |= 0x10;
 739         WREG_DAC(MGA1064_GEN_IO_CTL, tmp);
 740
 741         /* we are putting a 1 on the misc<0> line */
 742         WREG8(DAC_INDEX, MGA1064_GEN_IO_DATA);
 743         tmp = RREG8(DAC_DATA);
 744         tmp |= 0x10;
 745         WREG_DAC(MGA1064_GEN_IO_DATA, tmp);
 746
 747         /* 2- Second step to mask and further scan request
 748          * This will be done by asserting the remfreqmsk bit (XSPAREREG<7>)
 749          */
 750         WREG8(DAC_INDEX, MGA1064_SPAREREG);
 751         tmp = RREG8(DAC_DATA);
 752         tmp |= 0x80;
 753         WREG_DAC(MGA1064_SPAREREG, tmp);
 754
 755         /* 3a- the third step is to verifu if there is an active scan
 756          * We are searching for a 0 on remhsyncsts <XSPAREREG<0>)
 757          */
 758         iter_max = 300;
 759         while (!(tmp & 0x1) && iter_max) {
 760                 WREG8(DAC_INDEX, MGA1064_SPAREREG);
 761                 tmp = RREG8(DAC_DATA);
 762                 udelay(1000);
 763                 iter_max--;
 764         }
 765
 766         /* 3b- this step occurs only if the remove is actually scanning
 767          * we are waiting for the end of the frame which is a 1 on
 768          * remvsyncsts (XSPAREREG<1>)
 769          */
 770         if (iter_max) {
 771                 iter_max = 300;
 772                 while ((tmp & 0x2) && iter_max) {
 773                         WREG8(DAC_INDEX, MGA1064_SPAREREG);
 774                         tmp = RREG8(DAC_DATA);
 775                         udelay(1000);
 776                         iter_max--;
 777                 }
 778         }
 779 }
 780
 781 static void mga_g200wb_commit(struct drm_crtc *crtc)
 782 {
 783         u8 tmp;
 784         struct mga_device *mdev = crtc->dev->dev_private;
 785
 786         /* 1- The first step is to ensure that the vrsten and hrsten are set */
 787         WREG8(MGAREG_CRTCEXT_INDEX, 1);
 788         tmp = RREG8(MGAREG_CRTCEXT_DATA);
 789         WREG8(MGAREG_CRTCEXT_DATA, tmp | 0x88);
 790
 791         /* 2- second step is to assert the rstlvl2 */
 792         WREG8(DAC_INDEX, MGA1064_REMHEADCTL2);
 793         tmp = RREG8(DAC_DATA);
 794         tmp |= 0x8;
 795         WREG8(DAC_DATA, tmp);
 796
 797         /* wait 10 us */
 798         udelay(10);
 799
 800         /* 3- deassert rstlvl2 */
 801         tmp &= ~0x08;
 802         WREG8(DAC_INDEX, MGA1064_REMHEADCTL2);
 803         WREG8(DAC_DATA, tmp);
 804
 805         /* 4- remove mask of scan request */
 806         WREG8(DAC_INDEX, MGA1064_SPAREREG);
 807         tmp = RREG8(DAC_DATA);
 808         tmp &= ~0x80;
 809         WREG8(DAC_DATA, tmp);
 810
 811         /* 5- put back a 0 on the misc<0> line */
 812         WREG8(DAC_INDEX, MGA1064_GEN_IO_DATA);
 813         tmp = RREG8(DAC_DATA);
 814         tmp &= ~0x10;
 815         WREG_DAC(MGA1064_GEN_IO_DATA, tmp);
 816 }
 817
 818 /*
 819    This is how the framebuffer base address is stored in g200 cards:
 820    * Assume @offset is the gpu_addr variable of the framebuffer object
 821    * Then addr is the number of _pixels_ (not bytes) from the start of
 822      VRAM to the first pixel we want to display. (divided by 2 for 32bit
 823      framebuffers)
 824    * addr is stored in the CRTCEXT0, CRTCC and CRTCD registers
 825    addr<20> -> CRTCEXT0<6>
 826    addr<19-16> -> CRTCEXT0<3-0>
 827    addr<15-8> -> CRTCC<7-0>
 828    addr<7-0> -> CRTCD<7-0>
 829    CRTCEXT0 has to be programmed last to trigger an update and make the
 830    new addr variable take effect.
 831  */
 832 static void mga_set_start_address(struct drm_crtc *crtc, unsigned offset)
 833 {
 834         struct mga_device *mdev = crtc->dev->dev_private;
 835         u32 addr;
 836         int count;
 837         u8 crtcext0;
 838
 839         while (RREG8(0x1fda) & 0x08);
 840         while (!(RREG8(0x1fda) & 0x08));
 841
 842         count = RREG8(MGAREG_VCOUNT) + 2;
 843         while (RREG8(MGAREG_VCOUNT) < count);
 844
 845         WREG8(MGAREG_CRTCEXT_INDEX, 0);
 846         crtcext0 = RREG8(MGAREG_CRTCEXT_DATA);
 847         crtcext0 &= 0xB0;
 848         addr = offset / 8;
 849         /* Can't store addresses any higher than that...
 850            but we also don't have more than 16MB of memory, so it should be fine. */
 851         WARN_ON(addr > 0x1fffff);
 852         crtcext0 |= (!!(addr & (1<<20)))<<6;
 853         WREG_CRT(0x0d, (u8)(addr & 0xff));
 854         WREG_CRT(0x0c, (u8)(addr >> 8) & 0xff);
 855         WREG_ECRT(0x0, ((u8)(addr >> 16) & 0xf) | crtcext0);
 856 }
 857
 858 static int mga_crtc_do_set_base(struct drm_crtc *crtc,
 859                                 struct drm_framebuffer *fb,
 860                                 int x, int y, int atomic)
 861 {
 862         struct mga_device *mdev = crtc->dev->dev_private;
 863         struct drm_gem_object *obj;
 864         struct mga_framebuffer *mga_fb;
 865         struct drm_gem_vram_object *gbo;
 866         int ret;
 867         s64 gpu_addr;
 868         void *base;
 869
 870         if (!atomic && fb) {
 871                 mga_fb = to_mga_framebuffer(fb);
 872                 obj = mga_fb->obj;
 873                 gbo = drm_gem_vram_of_gem(obj);
 874
 875                 /* unmap if console */
 876                 if (&mdev->mfbdev->mfb == mga_fb)
 877                         drm_gem_vram_kunmap(gbo);
 878                 drm_gem_vram_unpin(gbo);
 879         }
 880
 881         mga_fb = to_mga_framebuffer(crtc->primary->fb);
 882         obj = mga_fb->obj;
 883         gbo = drm_gem_vram_of_gem(obj);
 884
 885         ret = drm_gem_vram_pin(gbo, DRM_GEM_VRAM_PL_FLAG_VRAM);
 886         if (ret)
 887                 return ret;
 888         gpu_addr = drm_gem_vram_offset(gbo);
 889         if (gpu_addr < 0) {
 890                 ret = (int)gpu_addr;
 891                 goto err_drm_gem_vram_unpin;
 892         }
 893
 894         if (&mdev->mfbdev->mfb == mga_fb) {
 895                 /* if pushing console in kmap it */
 896                 base = drm_gem_vram_kmap(gbo, true, NULL);
 897                 if (IS_ERR(base)) {
 898                         ret = PTR_ERR(base);
 899                         DRM_ERROR("failed to kmap fbcon\n");
 900                 }
 901         }
 902
 903         mga_set_start_address(crtc, (u32)gpu_addr);
 904
 905         return 0;
 906
 907 err_drm_gem_vram_unpin:
 908         drm_gem_vram_unpin(gbo);
 909         return ret;
 910 }
 911
 912 static int mga_crtc_mode_set_base(struct drm_crtc *crtc, int x, int y,
 913                                   struct drm_framebuffer *old_fb)
 914 {
 915         return mga_crtc_do_set_base(crtc, old_fb, x, y, 0);
 916 }
 917
 918 static int mga_crtc_mode_set(struct drm_crtc *crtc,
 919                                 struct drm_display_mode *mode,
 920                                 struct drm_display_mode *adjusted_mode,
 921                                 int x, int y, struct drm_framebuffer *old_fb)
 922 {
 923         struct drm_device *dev = crtc->dev;
 924         struct mga_device *mdev = dev->dev_private;
 925         const struct drm_framebuffer *fb = crtc->primary->fb;
 926         int hdisplay, hsyncstart, hsyncend, htotal;
 927         int vdisplay, vsyncstart, vsyncend, vtotal;
 928         int pitch;
 929         int option = 0, option2 = 0;
 930         int i;
 931         unsigned char misc = 0;
 932         unsigned char ext_vga[6];
 933         u8 bppshift;
 934
 935         static unsigned char dacvalue[] = {
 936                 /* 0x00: */        0,    0,    0,    0,    0,    0, 0x00,    0,
 937                 /* 0x08: */        0,    0,    0,    0,    0,    0,    0,    0,
 938                 /* 0x10: */        0,    0,    0,    0,    0,    0,    0,    0,
 939                 /* 0x18: */     0x00,    0, 0xC9, 0xFF, 0xBF, 0x20, 0x1F, 0x20,
 940                 /* 0x20: */     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 941                 /* 0x28: */     0x00, 0x00, 0x00, 0x00,    0,    0,    0, 0x40,
 942                 /* 0x30: */     0x00, 0xB0, 0x00, 0xC2, 0x34, 0x14, 0x02, 0x83,
 943                 /* 0x38: */     0x00, 0x93, 0x00, 0x77, 0x00, 0x00, 0x00, 0x3A,
 944                 /* 0x40: */        0,    0,    0,    0,    0,    0,    0,    0,
 945                 /* 0x48: */        0,    0,    0,    0,    0,    0,    0,    0
 946         };
 947
 948         bppshift = mdev->bpp_shifts[fb->format->cpp[0] - 1];
 949
 950         switch (mdev->type) {
 951         case G200_SE_A:
 952         case G200_SE_B:
 953                 dacvalue[MGA1064_VREF_CTL] = 0x03;
 954                 dacvalue[MGA1064_PIX_CLK_CTL] = MGA1064_PIX_CLK_CTL_SEL_PLL;
 955                 dacvalue[MGA1064_MISC_CTL] = MGA1064_MISC_CTL_DAC_EN |
 956                                              MGA1064_MISC_CTL_VGA8 |
 957                                              MGA1064_MISC_CTL_DAC_RAM_CS;
 958                 if (mdev->has_sdram)
 959                         option = 0x40049120;
 960                 else
 961                         option = 0x4004d120;
 962                 option2 = 0x00008000;
 963                 break;
 964         case G200_WB:
 965         case G200_EW3:
 966                 dacvalue[MGA1064_VREF_CTL] = 0x07;
 967                 option = 0x41049120;
 968                 option2 = 0x0000b000;
 969                 break;
 970         case G200_EV:
 971                 dacvalue[MGA1064_PIX_CLK_CTL] = MGA1064_PIX_CLK_CTL_SEL_PLL;
 972                 dacvalue[MGA1064_MISC_CTL] = MGA1064_MISC_CTL_VGA8 |
 973                                              MGA1064_MISC_CTL_DAC_RAM_CS;
 974                 option = 0x00000120;
 975                 option2 = 0x0000b000;
 976                 break;
 977         case G200_EH:
 978         case G200_EH3:
 979                 dacvalue[MGA1064_MISC_CTL] = MGA1064_MISC_CTL_VGA8 |
 980                                              MGA1064_MISC_CTL_DAC_RAM_CS;
 981                 option = 0x00000120;
 982                 option2 = 0x0000b000;
 983                 break;
 984         case G200_ER:
 985                 break;
 986         }
 987
 988         switch (fb->format->cpp[0] * 8) {
 989         case 8:
 990                 dacvalue[MGA1064_MUL_CTL] = MGA1064_MUL_CTL_8bits;
 991                 break;
 992         case 16:
 993                 if (fb->format->depth == 15)
 994                         dacvalue[MGA1064_MUL_CTL] = MGA1064_MUL_CTL_15bits;
 995                 else
 996                         dacvalue[MGA1064_MUL_CTL] = MGA1064_MUL_CTL_16bits;
 997                 break;
 998         case 24:
 999                 dacvalue[MGA1064_MUL_CTL] = MGA1064_MUL_CTL_24bits;
1000                 break;
1001         case 32:
1002                 dacvalue[MGA1064_MUL_CTL] = MGA1064_MUL_CTL_32_24bits;
1003                 break;
1004         }
1005
1006         if (mode->flags & DRM_MODE_FLAG_NHSYNC)
1007                 misc |= 0x40;
1008         if (mode->flags & DRM_MODE_FLAG_NVSYNC)
1009                 misc |= 0x80;
1010
1011
1012         for (i = 0; i < sizeof(dacvalue); i++) {
1013                 if ((i <= 0x17) ||
1014                     (i == 0x1b) ||
1015                     (i == 0x1c) ||
1016                     ((i >= 0x1f) && (i <= 0x29)) ||
1017                     ((i >= 0x30) && (i <= 0x37)))
1018                         continue;
1019                 if (IS_G200_SE(mdev) &&
1020                     ((i == 0x2c) || (i == 0x2d) || (i == 0x2e)))
1021                         continue;
1022                 if ((mdev->type == G200_EV ||
1023                     mdev->type == G200_WB ||
1024                     mdev->type == G200_EH ||
1025                     mdev->type == G200_EW3 ||
1026                     mdev->type == G200_EH3) &&
1027                     (i >= 0x44) && (i <= 0x4e))
1028                         continue;
1029
1030                 WREG_DAC(i, dacvalue[i]);
1031         }
1032
1033         if (mdev->type == G200_ER)
1034                 WREG_DAC(0x90, 0);
1035
1036         if (option)
1037                 pci_write_config_dword(dev->pdev, PCI_MGA_OPTION, option);
1038         if (option2)
1039                 pci_write_config_dword(dev->pdev, PCI_MGA_OPTION2, option2);
1040
1041         WREG_SEQ(2, 0xf);
1042         WREG_SEQ(3, 0);
1043         WREG_SEQ(4, 0xe);
1044
1045         pitch = fb->pitches[0] / fb->format->cpp[0];
1046         if (fb->format->cpp[0] * 8 == 24)
1047                 pitch = (pitch * 3) >> (4 - bppshift);
1048         else
1049                 pitch = pitch >> (4 - bppshift);
1050
1051         hdisplay = mode->hdisplay / 8 - 1;
1052         hsyncstart = mode->hsync_start / 8 - 1;
1053         hsyncend = mode->hsync_end / 8 - 1;
1054         htotal = mode->htotal / 8 - 1;
1055
1056         /* Work around hardware quirk */
1057         if ((htotal & 0x07) == 0x06 || (htotal & 0x07) == 0x04)
1058                 htotal++;
1059
1060         vdisplay = mode->vdisplay - 1;
1061         vsyncstart = mode->vsync_start - 1;
1062         vsyncend = mode->vsync_end - 1;
1063         vtotal = mode->vtotal - 2;
1064
1065         WREG_GFX(0, 0);
1066         WREG_GFX(1, 0);
1067         WREG_GFX(2, 0);
1068         WREG_GFX(3, 0);
1069         WREG_GFX(4, 0);
1070         WREG_GFX(5, 0x40);
1071         WREG_GFX(6, 0x5);
1072         WREG_GFX(7, 0xf);
1073         WREG_GFX(8, 0xf);
1074
1075         WREG_CRT(0, htotal - 4);
1076         WREG_CRT(1, hdisplay);
1077         WREG_CRT(2, hdisplay);
1078         WREG_CRT(3, (htotal & 0x1F) | 0x80);
1079         WREG_CRT(4, hsyncstart);
1080         WREG_CRT(5, ((htotal & 0x20) << 2) | (hsyncend & 0x1F));
1081         WREG_CRT(6, vtotal & 0xFF);
1082         WREG_CRT(7, ((vtotal & 0x100) >> 8) |
1083                  ((vdisplay & 0x100) >> 7) |
1084                  ((vsyncstart & 0x100) >> 6) |
1085                  ((vdisplay & 0x100) >> 5) |
1086                  ((vdisplay & 0x100) >> 4) | /* linecomp */
1087                  ((vtotal & 0x200) >> 4)|
1088                  ((vdisplay & 0x200) >> 3) |
1089                  ((vsyncstart & 0x200) >> 2));
1090         WREG_CRT(9, ((vdisplay & 0x200) >> 4) |
1091                  ((vdisplay & 0x200) >> 3));
1092         WREG_CRT(10, 0);
1093         WREG_CRT(11, 0);
1094         WREG_CRT(12, 0);
1095         WREG_CRT(13, 0);
1096         WREG_CRT(14, 0);
1097         WREG_CRT(15, 0);
1098         WREG_CRT(16, vsyncstart & 0xFF);
1099         WREG_CRT(17, (vsyncend & 0x0F) | 0x20);
1100         WREG_CRT(18, vdisplay & 0xFF);
1101         WREG_CRT(19, pitch & 0xFF);
1102         WREG_CRT(20, 0);
1103         WREG_CRT(21, vdisplay & 0xFF);
1104         WREG_CRT(22, (vtotal + 1) & 0xFF);
1105         WREG_CRT(23, 0xc3);
1106         WREG_CRT(24, vdisplay & 0xFF);
1107
1108         ext_vga[0] = 0;
1109         ext_vga[5] = 0;
1110
1111         /* TODO interlace */
1112
1113         ext_vga[0] |= (pitch & 0x300) >> 4;
1114         ext_vga[1] = (((htotal - 4) & 0x100) >> 8) |
1115                 ((hdisplay & 0x100) >> 7) |
1116                 ((hsyncstart & 0x100) >> 6) |
1117                 (htotal & 0x40);
1118         ext_vga[2] = ((vtotal & 0xc00) >> 10) |
1119                 ((vdisplay & 0x400) >> 8) |
1120                 ((vdisplay & 0xc00) >> 7) |
1121                 ((vsyncstart & 0xc00) >> 5) |
1122                 ((vdisplay & 0x400) >> 3);
1123         if (fb->format->cpp[0] * 8 == 24)
1124                 ext_vga[3] = (((1 << bppshift) * 3) - 1) | 0x80;
1125         else
1126                 ext_vga[3] = ((1 << bppshift) - 1) | 0x80;
1127         ext_vga[4] = 0;
1128         if (mdev->type == G200_WB || mdev->type == G200_EW3)
1129                 ext_vga[1] |= 0x88;
1130
1131         /* Set pixel clocks */
1132         misc = 0x2d;
1133         WREG8(MGA_MISC_OUT, misc);
1134
1135         mga_crtc_set_plls(mdev, mode->clock);
1136
1137         for (i = 0; i < 6; i++) {
1138                 WREG_ECRT(i, ext_vga[i]);
1139         }
1140
1141         if (mdev->type == G200_ER)
1142                 WREG_ECRT(0x24, 0x5);
1143
1144         if (mdev->type == G200_EW3)
1145                 WREG_ECRT(0x34, 0x5);
1146
1147         if (mdev->type == G200_EV) {
1148                 WREG_ECRT(6, 0);
1149         }
1150
1151         WREG_ECRT(0, ext_vga[0]);
1152         /* Enable mga pixel clock */
1153         misc = 0x2d;
1154
1155         WREG8(MGA_MISC_OUT, misc);
1156
1157         if (adjusted_mode)
1158                 memcpy(&mdev->mode, mode, sizeof(struct drm_display_mode));
1159
1160         mga_crtc_do_set_base(crtc, old_fb, x, y, 0);
1161
1162         /* reset tagfifo */
1163         if (mdev->type == G200_ER) {
1164                 u32 mem_ctl = RREG32(MGAREG_MEMCTL);
1165                 u8 seq1;
1166
1167                 /* screen off */
1168                 WREG8(MGAREG_SEQ_INDEX, 0x01);
1169                 seq1 = RREG8(MGAREG_SEQ_DATA) | 0x20;
1170                 WREG8(MGAREG_SEQ_DATA, seq1);
1171
1172                 WREG32(MGAREG_MEMCTL, mem_ctl | 0x00200000);
1173                 udelay(1000);
1174                 WREG32(MGAREG_MEMCTL, mem_ctl & ~0x00200000);
1175
1176                 WREG8(MGAREG_SEQ_DATA, seq1 & ~0x20);
1177         }
1178
1179
1180         if (IS_G200_SE(mdev)) {
1181                 if  (mdev->unique_rev_id >= 0x04) {
1182                         WREG8(MGAREG_CRTCEXT_INDEX, 0x06);
1183                         WREG8(MGAREG_CRTCEXT_DATA, 0);
1184                 } else if (mdev->unique_rev_id >= 0x02) {
1185                         u8 hi_pri_lvl;
1186                         u32 bpp;
1187                         u32 mb;
1188
1189                         if (fb->format->cpp[0] * 8 > 16)
1190                                 bpp = 32;
1191                         else if (fb->format->cpp[0] * 8 > 8)
1192                                 bpp = 16;
1193                         else
1194                                 bpp = 8;
1195
1196                         mb = (mode->clock * bpp) / 1000;
1197                         if (mb > 3100)
1198                                 hi_pri_lvl = 0;
1199                         else if (mb > 2600)
1200                                 hi_pri_lvl = 1;
1201                         else if (mb > 1900)
1202                                 hi_pri_lvl = 2;
1203                         else if (mb > 1160)
1204                                 hi_pri_lvl = 3;
1205                         else if (mb > 440)
1206                                 hi_pri_lvl = 4;
1207                         else
1208                                 hi_pri_lvl = 5;
1209
1210                         WREG8(MGAREG_CRTCEXT_INDEX, 0x06);
1211                         WREG8(MGAREG_CRTCEXT_DATA, hi_pri_lvl);
1212                 } else {
1213                         WREG8(MGAREG_CRTCEXT_INDEX, 0x06);
1214                         if (mdev->unique_rev_id >= 0x01)
1215                                 WREG8(MGAREG_CRTCEXT_DATA, 0x03);
1216                         else
1217                                 WREG8(MGAREG_CRTCEXT_DATA, 0x04);
1218                 }
1219         }
1220         return 0;
1221 }
1222
1223 #if 0 /* code from mjg to attempt D3 on crtc dpms off - revisit later */
1224 static int mga_suspend(struct drm_crtc *crtc)
1225 {
1226         struct mga_crtc *mga_crtc = to_mga_crtc(crtc);
1227         struct drm_device *dev = crtc->dev;
1228         struct mga_device *mdev = dev->dev_private;
1229         struct pci_dev *pdev = dev->pdev;
1230         int option;
1231
1232         if (mdev->suspended)
1233                 return 0;
1234
1235         WREG_SEQ(1, 0x20);
1236         WREG_ECRT(1, 0x30);
1237         /* Disable the pixel clock */
1238         WREG_DAC(0x1a, 0x05);
1239         /* Power down the DAC */
1240         WREG_DAC(0x1e, 0x18);
1241         /* Power down the pixel PLL */
1242         WREG_DAC(0x1a, 0x0d);
1243
1244         /* Disable PLLs and clocks */
1245         pci_read_config_dword(pdev, PCI_MGA_OPTION, &option);
1246         option &= ~(0x1F8024);
1247         pci_write_config_dword(pdev, PCI_MGA_OPTION, option);
1248         pci_set_power_state(pdev, PCI_D3hot);
1249         pci_disable_device(pdev);
1250
1251         mdev->suspended = true;
1252
1253         return 0;
1254 }
1255
1256 static int mga_resume(struct drm_crtc *crtc)
1257 {
1258         struct mga_crtc *mga_crtc = to_mga_crtc(crtc);
1259         struct drm_device *dev = crtc->dev;
1260         struct mga_device *mdev = dev->dev_private;
1261         struct pci_dev *pdev = dev->pdev;
1262         int option;
1263
1264         if (!mdev->suspended)
1265                 return 0;
1266
1267         pci_set_power_state(pdev, PCI_D0);
1268         pci_enable_device(pdev);
1269
1270         /* Disable sysclk */
1271         pci_read_config_dword(pdev, PCI_MGA_OPTION, &option);
1272         option &= ~(0x4);
1273         pci_write_config_dword(pdev, PCI_MGA_OPTION, option);
1274
1275         mdev->suspended = false;
1276
1277         return 0;
1278 }
1279
1280 #endif
1281
1282 static void mga_crtc_dpms(struct drm_crtc *crtc, int mode)
1283 {
1284         struct drm_device *dev = crtc->dev;
1285         struct mga_device *mdev = dev->dev_private;
1286         u8 seq1 = 0, crtcext1 = 0;
1287
1288         switch (mode) {
1289         case DRM_MODE_DPMS_ON:
1290                 seq1 = 0;
1291                 crtcext1 = 0;
1292                 mga_crtc_load_lut(crtc);
1293                 break;
1294         case DRM_MODE_DPMS_STANDBY:
1295                 seq1 = 0x20;
1296                 crtcext1 = 0x10;
1297                 break;
1298         case DRM_MODE_DPMS_SUSPEND:
1299                 seq1 = 0x20;
1300                 crtcext1 = 0x20;
1301                 break;
1302         case DRM_MODE_DPMS_OFF:
1303                 seq1 = 0x20;
1304                 crtcext1 = 0x30;
1305                 break;
1306         }
1307
1308 #if 0
1309         if (mode == DRM_MODE_DPMS_OFF) {
1310                 mga_suspend(crtc);
1311         }
1312 #endif
1313         WREG8(MGAREG_SEQ_INDEX, 0x01);
1314         seq1 |= RREG8(MGAREG_SEQ_DATA) & ~0x20;
1315         mga_wait_vsync(mdev);
1316         mga_wait_busy(mdev);
1317         WREG8(MGAREG_SEQ_DATA, seq1);
1318         msleep(20);
1319         WREG8(MGAREG_CRTCEXT_INDEX, 0x01);
1320         crtcext1 |= RREG8(MGAREG_CRTCEXT_DATA) & ~0x30;
1321         WREG8(MGAREG_CRTCEXT_DATA, crtcext1);
1322
1323 #if 0
1324         if (mode == DRM_MODE_DPMS_ON && mdev->suspended == true) {
1325                 mga_resume(crtc);
1326                 drm_helper_resume_force_mode(dev);
1327         }
1328 #endif
1329 }
1330
1331 /*
1332  * This is called before a mode is programmed. A typical use might be to
1333  * enable DPMS during the programming to avoid seeing intermediate stages,
1334  * but that's not relevant to us
1335  */
1336 static void mga_crtc_prepare(struct drm_crtc *crtc)
1337 {
1338         struct drm_device *dev = crtc->dev;
1339         struct mga_device *mdev = dev->dev_private;
1340         u8 tmp;
1341
1342         /*      mga_resume(crtc);*/
1343
1344         WREG8(MGAREG_CRTC_INDEX, 0x11);
1345         tmp = RREG8(MGAREG_CRTC_DATA);
1346         WREG_CRT(0x11, tmp | 0x80);
1347
1348         if (mdev->type == G200_SE_A || mdev->type == G200_SE_B) {
1349                 WREG_SEQ(0, 1);
1350                 msleep(50);
1351                 WREG_SEQ(1, 0x20);
1352                 msleep(20);
1353         } else {
1354                 WREG8(MGAREG_SEQ_INDEX, 0x1);
1355                 tmp = RREG8(MGAREG_SEQ_DATA);
1356
1357                 /* start sync reset */
1358                 WREG_SEQ(0, 1);
1359                 WREG_SEQ(1, tmp | 0x20);
1360         }
1361
1362         if (mdev->type == G200_WB || mdev->type == G200_EW3)
1363                 mga_g200wb_prepare(crtc);
1364
1365         WREG_CRT(17, 0);
1366 }
1367
1368 /*
1369  * This is called after a mode is programmed. It should reverse anything done
1370  * by the prepare function
1371  */
1372 static void mga_crtc_commit(struct drm_crtc *crtc)
1373 {
1374         struct drm_device *dev = crtc->dev;
1375         struct mga_device *mdev = dev->dev_private;
1376         const struct drm_crtc_helper_funcs *crtc_funcs = crtc->helper_private;
1377         u8 tmp;
1378
1379         if (mdev->type == G200_WB || mdev->type == G200_EW3)
1380                 mga_g200wb_commit(crtc);
1381
1382         if (mdev->type == G200_SE_A || mdev->type == G200_SE_B) {
1383                 msleep(50);
1384                 WREG_SEQ(1, 0x0);
1385                 msleep(20);
1386                 WREG_SEQ(0, 0x3);
1387         } else {
1388                 WREG8(MGAREG_SEQ_INDEX, 0x1);
1389                 tmp = RREG8(MGAREG_SEQ_DATA);
1390
1391                 tmp &= ~0x20;
1392                 WREG_SEQ(0x1, tmp);
1393                 WREG_SEQ(0, 3);
1394         }
1395         crtc_funcs->dpms(crtc, DRM_MODE_DPMS_ON);
1396 }
1397
1398 /*
1399  * The core can pass us a set of gamma values to program. We actually only
1400  * use this for 8-bit mode so can't perform smooth fades on deeper modes,
1401  * but it's a requirement that we provide the function
1402  */
1403 static int mga_crtc_gamma_set(struct drm_crtc *crtc, u16 *red, u16 *green,
1404                               u16 *blue, uint32_t size,
1405                               struct drm_modeset_acquire_ctx *ctx)
1406 {
1407         mga_crtc_load_lut(crtc);
1408
1409         return 0;
1410 }
1411
1412 /* Simple cleanup function */
1413 static void mga_crtc_destroy(struct drm_crtc *crtc)
1414 {
1415         struct mga_crtc *mga_crtc = to_mga_crtc(crtc);
1416
1417         drm_crtc_cleanup(crtc);
1418         kfree(mga_crtc);
1419 }
1420
1421 static void mga_crtc_disable(struct drm_crtc *crtc)
1422 {
1423         DRM_DEBUG_KMS("\n");
1424         mga_crtc_dpms(crtc, DRM_MODE_DPMS_OFF);
1425         if (crtc->primary->fb) {
1426                 struct mga_device *mdev = crtc->dev->dev_private;
1427                 struct mga_framebuffer *mga_fb = to_mga_framebuffer(crtc->primary->fb);
1428                 struct drm_gem_object *obj = mga_fb->obj;
1429                 struct drm_gem_vram_object *gbo = drm_gem_vram_of_gem(obj);
1430
1431                 /* unmap if console */
1432                 if (&mdev->mfbdev->mfb == mga_fb)
1433                         drm_gem_vram_kunmap(gbo);
1434                 drm_gem_vram_unpin(gbo);
1435         }
1436         crtc->primary->fb = NULL;
1437 }
1438
1439 /* These provide the minimum set of functions required to handle a CRTC */
1440 static const struct drm_crtc_funcs mga_crtc_funcs = {
1441         .cursor_set = mga_crtc_cursor_set,
1442         .cursor_move = mga_crtc_cursor_move,
1443         .gamma_set = mga_crtc_gamma_set,
1444         .set_config = drm_crtc_helper_set_config,
1445         .destroy = mga_crtc_destroy,
1446 };
1447
1448 static const struct drm_crtc_helper_funcs mga_helper_funcs = {
1449         .disable = mga_crtc_disable,
1450         .dpms = mga_crtc_dpms,
1451         .mode_set = mga_crtc_mode_set,
1452         .mode_set_base = mga_crtc_mode_set_base,
1453         .prepare = mga_crtc_prepare,
1454         .commit = mga_crtc_commit,
1455 };
1456
1457 /* CRTC setup */
1458 static void mga_crtc_init(struct mga_device *mdev)
1459 {
1460         struct mga_crtc *mga_crtc;
1461
1462         mga_crtc = kzalloc(sizeof(struct mga_crtc) +
1463                               (MGAG200FB_CONN_LIMIT * sizeof(struct drm_connector *)),
1464                               GFP_KERNEL);
1465
1466         if (mga_crtc == NULL)
1467                 return;
1468
1469         drm_crtc_init(mdev->dev, &mga_crtc->base, &mga_crtc_funcs);
1470
1471         drm_mode_crtc_set_gamma_size(&mga_crtc->base, MGAG200_LUT_SIZE);
1472         mdev->mode_info.crtc = mga_crtc;
1473
1474         drm_crtc_helper_add(&mga_crtc->base, &mga_helper_funcs);
1475 }
1476
1477 /*
1478  * The encoder comes after the CRTC in the output pipeline, but before
1479  * the connector. It's responsible for ensuring that the digital
1480  * stream is appropriately converted into the output format. Setup is
1481  * very simple in this case - all we have to do is inform qemu of the
1482  * colour depth in order to ensure that it displays appropriately
1483  */
1484
1485 /*
1486  * These functions are analagous to those in the CRTC code, but are intended
1487  * to handle any encoder-specific limitations
1488  */
1489 static void mga_encoder_mode_set(struct drm_encoder *encoder,
1490                                 struct drm_display_mode *mode,
1491                                 struct drm_display_mode *adjusted_mode)
1492 {
1493
1494 }
1495
1496 static void mga_encoder_dpms(struct drm_encoder *encoder, int state)
1497 {
1498         return;
1499 }
1500
1501 static void mga_encoder_prepare(struct drm_encoder *encoder)
1502 {
1503 }
1504
1505 static void mga_encoder_commit(struct drm_encoder *encoder)
1506 {
1507 }
1508
1509 static void mga_encoder_destroy(struct drm_encoder *encoder)
1510 {
1511         struct mga_encoder *mga_encoder = to_mga_encoder(encoder);
1512         drm_encoder_cleanup(encoder);
1513         kfree(mga_encoder);
1514 }
1515
1516 static const struct drm_encoder_helper_funcs mga_encoder_helper_funcs = {
1517         .dpms = mga_encoder_dpms,
1518         .mode_set = mga_encoder_mode_set,
1519         .prepare = mga_encoder_prepare,
1520         .commit = mga_encoder_commit,
1521 };
1522
1523 static const struct drm_encoder_funcs mga_encoder_encoder_funcs = {
1524         .destroy = mga_encoder_destroy,
1525 };
1526
1527 static struct drm_encoder *mga_encoder_init(struct drm_device *dev)
1528 {
1529         struct drm_encoder *encoder;
1530         struct mga_encoder *mga_encoder;
1531
1532         mga_encoder = kzalloc(sizeof(struct mga_encoder), GFP_KERNEL);
1533         if (!mga_encoder)
1534                 return NULL;
1535
1536         encoder = &mga_encoder->base;
1537         encoder->possible_crtcs = 0x1;
1538
1539         drm_encoder_init(dev, encoder, &mga_encoder_encoder_funcs,
1540                          DRM_MODE_ENCODER_DAC, NULL);
1541         drm_encoder_helper_add(encoder, &mga_encoder_helper_funcs);
1542
1543         return encoder;
1544 }
1545
1546
1547 static int mga_vga_get_modes(struct drm_connector *connector)
1548 {
1549         struct mga_connector *mga_connector = to_mga_connector(connector);
1550         struct edid *edid;
1551         int ret = 0;
1552
1553         edid = drm_get_edid(connector, &mga_connector->i2c->adapter);
1554         if (edid) {
1555                 drm_connector_update_edid_property(connector, edid);
1556                 ret = drm_add_edid_modes(connector, edid);
1557                 kfree(edid);
1558         }
1559         return ret;
1560 }
1561
1562 static uint32_t mga_vga_calculate_mode_bandwidth(struct drm_display_mode *mode,
1563                                                         int bits_per_pixel)
1564 {
1565         uint32_t total_area, divisor;
1566         uint64_t active_area, pixels_per_second, bandwidth;
1567         uint64_t bytes_per_pixel = (bits_per_pixel + 7) / 8;
1568
1569         divisor = 1024;
1570
1571         if (!mode->htotal || !mode->vtotal || !mode->clock)
1572                 return 0;
1573
1574         active_area = mode->hdisplay * mode->vdisplay;
1575         total_area = mode->htotal * mode->vtotal;
1576
1577         pixels_per_second = active_area * mode->clock * 1000;
1578         do_div(pixels_per_second, total_area);
1579
1580         bandwidth = pixels_per_second * bytes_per_pixel * 100;
1581         do_div(bandwidth, divisor);
1582
1583         return (uint32_t)(bandwidth);
1584 }
1585
1586 #define MODE_BANDWIDTH  MODE_BAD
1587
1588 static enum drm_mode_status mga_vga_mode_valid(struct drm_connector *connector,
1589                                  struct drm_display_mode *mode)
1590 {
1591         struct drm_device *dev = connector->dev;
1592         struct mga_device *mdev = (struct mga_device*)dev->dev_private;
1593         int bpp = 32;
1594
1595         if (IS_G200_SE(mdev)) {
1596                 if (mdev->unique_rev_id == 0x01) {
1597                         if (mode->hdisplay > 1600)
1598                                 return MODE_VIRTUAL_X;
1599                         if (mode->vdisplay > 1200)
1600                                 return MODE_VIRTUAL_Y;
1601                         if (mga_vga_calculate_mode_bandwidth(mode, bpp)
1602                                 > (24400 * 1024))
1603                                 return MODE_BANDWIDTH;
1604                 } else if (mdev->unique_rev_id == 0x02) {
1605                         if (mode->hdisplay > 1920)
1606                                 return MODE_VIRTUAL_X;
1607                         if (mode->vdisplay > 1200)
1608                                 return MODE_VIRTUAL_Y;
1609                         if (mga_vga_calculate_mode_bandwidth(mode, bpp)
1610                                 > (30100 * 1024))
1611                                 return MODE_BANDWIDTH;
1612                 } else {
1613                         if (mga_vga_calculate_mode_bandwidth(mode, bpp)
1614                                 > (55000 * 1024))
1615                                 return MODE_BANDWIDTH;
1616                 }
1617         } else if (mdev->type == G200_WB) {
1618                 if (mode->hdisplay > 1280)
1619                         return MODE_VIRTUAL_X;
1620                 if (mode->vdisplay > 1024)
1621                         return MODE_VIRTUAL_Y;
1622                 if (mga_vga_calculate_mode_bandwidth(mode, bpp) >
1623                     (31877 * 1024))
1624                         return MODE_BANDWIDTH;
1625         } else if (mdev->type == G200_EV &&
1626                 (mga_vga_calculate_mode_bandwidth(mode, bpp)
1627                         > (32700 * 1024))) {
1628                 return MODE_BANDWIDTH;
1629         } else if (mdev->type == G200_EH &&
1630                 (mga_vga_calculate_mode_bandwidth(mode, bpp)
1631                         > (37500 * 1024))) {
1632                 return MODE_BANDWIDTH;
1633         } else if (mdev->type == G200_ER &&
1634                 (mga_vga_calculate_mode_bandwidth(mode,
1635                         bpp) > (55000 * 1024))) {
1636                 return MODE_BANDWIDTH;
1637         }
1638
1639         if ((mode->hdisplay % 8) != 0 || (mode->hsync_start % 8) != 0 ||
1640             (mode->hsync_end % 8) != 0 || (mode->htotal % 8) != 0) {
1641                 return MODE_H_ILLEGAL;
1642         }
1643
1644         if (mode->crtc_hdisplay > 2048 || mode->crtc_hsync_start > 4096 ||
1645             mode->crtc_hsync_end > 4096 || mode->crtc_htotal > 4096 ||
1646             mode->crtc_vdisplay > 2048 || mode->crtc_vsync_start > 4096 ||
1647             mode->crtc_vsync_end > 4096 || mode->crtc_vtotal > 4096) {
1648                 return MODE_BAD;
1649         }
1650
1651         /* Validate the mode input by the user */
1652         if (connector->cmdline_mode.specified) {
1653                 if (connector->cmdline_mode.bpp_specified)
1654                         bpp = connector->cmdline_mode.bpp;
1655         }
1656
1657         if ((mode->hdisplay * mode->vdisplay * (bpp/8)) > mdev->mc.vram_size) {
1658                 if (connector->cmdline_mode.specified)
1659                         connector->cmdline_mode.specified = false;
1660                 return MODE_BAD;
1661         }
1662
1663         return MODE_OK;
1664 }
1665
1666 static struct drm_encoder *mga_connector_best_encoder(struct drm_connector
1667                                                   *connector)
1668 {
1669         int enc_id = connector->encoder_ids[0];
1670         /* pick the encoder ids */
1671         if (enc_id)
1672                 return drm_encoder_find(connector->dev, NULL, enc_id);
1673         return NULL;
1674 }
1675
1676 static void mga_connector_destroy(struct drm_connector *connector)
1677 {
1678         struct mga_connector *mga_connector = to_mga_connector(connector);
1679         mgag200_i2c_destroy(mga_connector->i2c);
1680         drm_connector_cleanup(connector);
1681         kfree(connector);
1682 }
1683
1684 static const struct drm_connector_helper_funcs mga_vga_connector_helper_funcs = {
1685         .get_modes = mga_vga_get_modes,
1686         .mode_valid = mga_vga_mode_valid,
1687         .best_encoder = mga_connector_best_encoder,
1688 };
1689
1690 static const struct drm_connector_funcs mga_vga_connector_funcs = {
1691         .dpms = drm_helper_connector_dpms,
1692         .fill_modes = drm_helper_probe_single_connector_modes,
1693         .destroy = mga_connector_destroy,
1694 };
1695
1696 static struct drm_connector *mga_vga_init(struct drm_device *dev)
1697 {
1698         struct drm_connector *connector;
1699         struct mga_connector *mga_connector;
1700
1701         mga_connector = kzalloc(sizeof(struct mga_connector), GFP_KERNEL);
1702         if (!mga_connector)
1703                 return NULL;
1704
1705         connector = &mga_connector->base;
1706
1707         drm_connector_init(dev, connector,
1708                            &mga_vga_connector_funcs, DRM_MODE_CONNECTOR_VGA);
1709
1710         drm_connector_helper_add(connector, &mga_vga_connector_helper_funcs);
1711
1712         drm_connector_register(connector);
1713
1714         mga_connector->i2c = mgag200_i2c_create(dev);
1715         if (!mga_connector->i2c)
1716                 DRM_ERROR("failed to add ddc bus\n");
1717
1718         return connector;
1719 }
1720
1721
1722 int mgag200_modeset_init(struct mga_device *mdev)
1723 {
1724         struct drm_encoder *encoder;
1725         struct drm_connector *connector;
1726         int ret;
1727
1728         mdev->mode_info.mode_config_initialized = true;
1729
1730         mdev->dev->mode_config.max_width = MGAG200_MAX_FB_WIDTH;
1731         mdev->dev->mode_config.max_height = MGAG200_MAX_FB_HEIGHT;
1732
1733         mdev->dev->mode_config.fb_base = mdev->mc.vram_base;
1734
1735         mga_crtc_init(mdev);
1736
1737         encoder = mga_encoder_init(mdev->dev);
1738         if (!encoder) {
1739                 DRM_ERROR("mga_encoder_init failed\n");
1740                 return -1;
1741         }
1742
1743         connector = mga_vga_init(mdev->dev);
1744         if (!connector) {
1745                 DRM_ERROR("mga_vga_init failed\n");
1746                 return -1;
1747         }
1748
1749         drm_connector_attach_encoder(connector, encoder);
1750
1751         ret = mgag200_fbdev_init(mdev);
1752         if (ret) {
1753                 DRM_ERROR("mga_fbdev_init failed\n");
1754                 return ret;
1755         }
1756
1757         return 0;
1758 }
1759
1760 void mgag200_modeset_fini(struct mga_device *mdev)
1761 {
1762
1763 }