drivers/usb/misc/sisusbvga/sisusb_init.c

   1 /*
   2  * sisusb - usb kernel driver for SiS315(E) based USB2VGA dongles
   3  *
   4  * Display mode initializing code
   5  *
   6  * Copyright (C) 2001-2005 by Thomas Winischhofer, Vienna, Austria
   7  *
   8  * If distributed as part of the Linux kernel, this code is licensed under the
   9  * terms of the GPL v2.
  10  *
  11  * Otherwise, the following license terms apply:
  12  *
  13  * * Redistribution and use in source and binary forms, with or without
  14  * * modification, are permitted provided that the following conditions
  15  * * are met:
  16  * * 1) Redistributions of source code must retain the above copyright
  17  * *    notice, this list of conditions and the following disclaimer.
  18  * * 2) Redistributions in binary form must reproduce the above copyright
  19  * *    notice, this list of conditions and the following disclaimer in the
  20  * *    documentation and/or other materials provided with the distribution.
  21  * * 3) The name of the author may not be used to endorse or promote products
  22  * *    derived from this software without specific prior written permission.
  23  * *
  24  * * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
  25  * * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
  26  * * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
  27  * * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
  28  * * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
  29  * * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
  30  * * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
  31  * * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
  32  * * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
  33  * * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  34  *
  35  * Author:      Thomas Winischhofer <thomas@winischhofer.net>
  36  *
  37  */
  38
  39 #include <linux/module.h>
  40 #include <linux/kernel.h>
  41 #include <linux/errno.h>
  42 #include <linux/poll.h>
  43 #include <linux/spinlock.h>
  44
  45 #include "sisusb.h"
  46
  47 #ifdef INCL_SISUSB_CON
  48
  49 #include "sisusb_init.h"
  50
  51 /*********************************************/
  52 /*         POINTER INITIALIZATION            */
  53 /*********************************************/
  54
  55 static void SiSUSB_InitPtr(struct SiS_Private *SiS_Pr)
  56 {
  57         SiS_Pr->SiS_ModeResInfo = SiSUSB_ModeResInfo;
  58         SiS_Pr->SiS_StandTable = SiSUSB_StandTable;
  59
  60         SiS_Pr->SiS_SModeIDTable = SiSUSB_SModeIDTable;
  61         SiS_Pr->SiS_EModeIDTable = SiSUSB_EModeIDTable;
  62         SiS_Pr->SiS_RefIndex = SiSUSB_RefIndex;
  63         SiS_Pr->SiS_CRT1Table = SiSUSB_CRT1Table;
  64
  65         SiS_Pr->SiS_VCLKData = SiSUSB_VCLKData;
  66 }
  67
  68 /*********************************************/
  69 /*          HELPER: SetReg, GetReg           */
  70 /*********************************************/
  71
  72 static void
  73 SiS_SetReg(struct SiS_Private *SiS_Pr, unsigned long port,
  74            unsigned short index, unsigned short data)
  75 {
  76         sisusb_setidxreg(SiS_Pr->sisusb, port, index, data);
  77 }
  78
  79 static void
  80 SiS_SetRegByte(struct SiS_Private *SiS_Pr, unsigned long port,
  81                unsigned short data)
  82 {
  83         sisusb_setreg(SiS_Pr->sisusb, port, data);
  84 }
  85
  86 static unsigned char
  87 SiS_GetReg(struct SiS_Private *SiS_Pr, unsigned long port, unsigned short index)
  88 {
  89         u8 data;
  90
  91         sisusb_getidxreg(SiS_Pr->sisusb, port, index, &data);
  92
  93         return data;
  94 }
  95
  96 static unsigned char
  97 SiS_GetRegByte(struct SiS_Private *SiS_Pr, unsigned long port)
  98 {
  99         u8 data;
 100
 101         sisusb_getreg(SiS_Pr->sisusb, port, &data);
 102
 103         return data;
 104 }
 105
 106 static void
 107 SiS_SetRegANDOR(struct SiS_Private *SiS_Pr, unsigned long port,
 108                 unsigned short index, unsigned short DataAND,
 109                 unsigned short DataOR)
 110 {
 111         sisusb_setidxregandor(SiS_Pr->sisusb, port, index, DataAND, DataOR);
 112 }
 113
 114 static void
 115 SiS_SetRegAND(struct SiS_Private *SiS_Pr, unsigned long port,
 116               unsigned short index, unsigned short DataAND)
 117 {
 118         sisusb_setidxregand(SiS_Pr->sisusb, port, index, DataAND);
 119 }
 120
 121 static void
 122 SiS_SetRegOR(struct SiS_Private *SiS_Pr, unsigned long port,
 123              unsigned short index, unsigned short DataOR)
 124 {
 125         sisusb_setidxregor(SiS_Pr->sisusb, port, index, DataOR);
 126 }
 127
 128 /*********************************************/
 129 /*      HELPER: DisplayOn, DisplayOff        */
 130 /*********************************************/
 131
 132 static void SiS_DisplayOn(struct SiS_Private *SiS_Pr)
 133 {
 134         SiS_SetRegAND(SiS_Pr, SiS_Pr->SiS_P3c4, 0x01, 0xDF);
 135 }
 136
 137 /*********************************************/
 138 /*        HELPER: Init Port Addresses        */
 139 /*********************************************/
 140
 141 static void SiSUSBRegInit(struct SiS_Private *SiS_Pr, unsigned long BaseAddr)
 142 {
 143         SiS_Pr->SiS_P3c4 = BaseAddr + 0x14;
 144         SiS_Pr->SiS_P3d4 = BaseAddr + 0x24;
 145         SiS_Pr->SiS_P3c0 = BaseAddr + 0x10;
 146         SiS_Pr->SiS_P3ce = BaseAddr + 0x1e;
 147         SiS_Pr->SiS_P3c2 = BaseAddr + 0x12;
 148         SiS_Pr->SiS_P3ca = BaseAddr + 0x1a;
 149         SiS_Pr->SiS_P3c6 = BaseAddr + 0x16;
 150         SiS_Pr->SiS_P3c7 = BaseAddr + 0x17;
 151         SiS_Pr->SiS_P3c8 = BaseAddr + 0x18;
 152         SiS_Pr->SiS_P3c9 = BaseAddr + 0x19;
 153         SiS_Pr->SiS_P3cb = BaseAddr + 0x1b;
 154         SiS_Pr->SiS_P3cc = BaseAddr + 0x1c;
 155         SiS_Pr->SiS_P3cd = BaseAddr + 0x1d;
 156         SiS_Pr->SiS_P3da = BaseAddr + 0x2a;
 157         SiS_Pr->SiS_Part1Port = BaseAddr + SIS_CRT2_PORT_04;
 158 }
 159
 160 /*********************************************/
 161 /*             HELPER: GetSysFlags           */
 162 /*********************************************/
 163
 164 static void SiS_GetSysFlags(struct SiS_Private *SiS_Pr)
 165 {
 166         SiS_Pr->SiS_MyCR63 = 0x63;
 167 }
 168
 169 /*********************************************/
 170 /*         HELPER: Init PCI & Engines        */
 171 /*********************************************/
 172
 173 static void SiSInitPCIetc(struct SiS_Private *SiS_Pr)
 174 {
 175         SiS_SetReg(SiS_Pr, SiS_Pr->SiS_P3c4, 0x20, 0xa1);
 176         /*  - Enable 2D (0x40)
 177          *  - Enable 3D (0x02)
 178          *  - Enable 3D vertex command fetch (0x10)
 179          *  - Enable 3D command parser (0x08)
 180          *  - Enable 3D G/L transformation engine (0x80)
 181          */
 182         SiS_SetRegOR(SiS_Pr, SiS_Pr->SiS_P3c4, 0x1E, 0xDA);
 183 }
 184
 185 /*********************************************/
 186 /*        HELPER: SET SEGMENT REGISTERS      */
 187 /*********************************************/
 188
 189 static void SiS_SetSegRegLower(struct SiS_Private *SiS_Pr, unsigned short value)
 190 {
 191         unsigned short temp;
 192
 193         value &= 0x00ff;
 194         temp = SiS_GetRegByte(SiS_Pr, SiS_Pr->SiS_P3cb) & 0xf0;
 195         temp |= (value >> 4);
 196         SiS_SetRegByte(SiS_Pr, SiS_Pr->SiS_P3cb, temp);
 197         temp = SiS_GetRegByte(SiS_Pr, SiS_Pr->SiS_P3cd) & 0xf0;
 198         temp |= (value & 0x0f);
 199         SiS_SetRegByte(SiS_Pr, SiS_Pr->SiS_P3cd, temp);
 200 }
 201
 202 static void SiS_SetSegRegUpper(struct SiS_Private *SiS_Pr, unsigned short value)
 203 {
 204         unsigned short temp;
 205
 206         value &= 0x00ff;
 207         temp = SiS_GetRegByte(SiS_Pr, SiS_Pr->SiS_P3cb) & 0x0f;
 208         temp |= (value & 0xf0);
 209         SiS_SetRegByte(SiS_Pr, SiS_Pr->SiS_P3cb, temp);
 210         temp = SiS_GetRegByte(SiS_Pr, SiS_Pr->SiS_P3cd) & 0x0f;
 211         temp |= (value << 4);
 212         SiS_SetRegByte(SiS_Pr, SiS_Pr->SiS_P3cd, temp);
 213 }
 214
 215 static void SiS_SetSegmentReg(struct SiS_Private *SiS_Pr, unsigned short value)
 216 {
 217         SiS_SetSegRegLower(SiS_Pr, value);
 218         SiS_SetSegRegUpper(SiS_Pr, value);
 219 }
 220
 221 static void SiS_ResetSegmentReg(struct SiS_Private *SiS_Pr)
 222 {
 223         SiS_SetSegmentReg(SiS_Pr, 0);
 224 }
 225
 226 static void
 227 SiS_SetSegmentRegOver(struct SiS_Private *SiS_Pr, unsigned short value)
 228 {
 229         unsigned short temp = value >> 8;
 230
 231         temp &= 0x07;
 232         temp |= (temp << 4);
 233         SiS_SetReg(SiS_Pr, SiS_Pr->SiS_P3c4, 0x1d, temp);
 234         SiS_SetSegmentReg(SiS_Pr, value);
 235 }
 236
 237 static void SiS_ResetSegmentRegOver(struct SiS_Private *SiS_Pr)
 238 {
 239         SiS_SetSegmentRegOver(SiS_Pr, 0);
 240 }
 241
 242 static void SiS_ResetSegmentRegisters(struct SiS_Private *SiS_Pr)
 243 {
 244         SiS_ResetSegmentReg(SiS_Pr);
 245         SiS_ResetSegmentRegOver(SiS_Pr);
 246 }
 247
 248 /*********************************************/
 249 /*           HELPER: SearchModeID            */
 250 /*********************************************/
 251
 252 static int
 253 SiS_SearchModeID(struct SiS_Private *SiS_Pr, unsigned short *ModeNo,
 254                  unsigned short *ModeIdIndex)
 255 {
 256         if ((*ModeNo) <= 0x13) {
 257
 258                 if ((*ModeNo) != 0x03)
 259                         return 0;
 260
 261                 (*ModeIdIndex) = 0;
 262
 263         } else {
 264
 265                 for (*ModeIdIndex = 0;; (*ModeIdIndex)++) {
 266
 267                         if (SiS_Pr->SiS_EModeIDTable[*ModeIdIndex].Ext_ModeID ==
 268                             (*ModeNo))
 269                                 break;
 270
 271                         if (SiS_Pr->SiS_EModeIDTable[*ModeIdIndex].Ext_ModeID ==
 272                             0xFF)
 273                                 return 0;
 274                 }
 275
 276         }
 277
 278         return 1;
 279 }
 280
 281 /*********************************************/
 282 /*            HELPER: ENABLE CRT1            */
 283 /*********************************************/
 284
 285 static void SiS_HandleCRT1(struct SiS_Private *SiS_Pr)
 286 {
 287         /* Enable CRT1 gating */
 288         SiS_SetRegAND(SiS_Pr, SiS_Pr->SiS_P3d4, SiS_Pr->SiS_MyCR63, 0xbf);
 289 }
 290
 291 /*********************************************/
 292 /*           HELPER: GetColorDepth           */
 293 /*********************************************/
 294
 295 static unsigned short
 296 SiS_GetColorDepth(struct SiS_Private *SiS_Pr, unsigned short ModeNo,
 297                   unsigned short ModeIdIndex)
 298 {
 299         static const unsigned short ColorDepth[6] = { 1, 2, 4, 4, 6, 8 };
 300         unsigned short modeflag;
 301         short index;
 302
 303         if (ModeNo <= 0x13) {
 304                 modeflag = SiS_Pr->SiS_SModeIDTable[ModeIdIndex].St_ModeFlag;
 305         } else {
 306                 modeflag = SiS_Pr->SiS_EModeIDTable[ModeIdIndex].Ext_ModeFlag;
 307         }
 308
 309         index = (modeflag & ModeTypeMask) - ModeEGA;
 310         if (index < 0)
 311                 index = 0;
 312         return ColorDepth[index];
 313 }
 314
 315 /*********************************************/
 316 /*             HELPER: GetOffset             */
 317 /*********************************************/
 318
 319 static unsigned short
 320 SiS_GetOffset(struct SiS_Private *SiS_Pr, unsigned short ModeNo,
 321               unsigned short ModeIdIndex, unsigned short rrti)
 322 {
 323         unsigned short xres, temp, colordepth, infoflag;
 324
 325         infoflag = SiS_Pr->SiS_RefIndex[rrti].Ext_InfoFlag;
 326         xres = SiS_Pr->SiS_RefIndex[rrti].XRes;
 327
 328         colordepth = SiS_GetColorDepth(SiS_Pr, ModeNo, ModeIdIndex);
 329
 330         temp = xres / 16;
 331
 332         if (infoflag & InterlaceMode)
 333                 temp <<= 1;
 334
 335         temp *= colordepth;
 336
 337         if (xres % 16)
 338                 temp += (colordepth >> 1);
 339
 340         return temp;
 341 }
 342
 343 /*********************************************/
 344 /*                   SEQ                     */
 345 /*********************************************/
 346
 347 static void
 348 SiS_SetSeqRegs(struct SiS_Private *SiS_Pr, unsigned short StandTableIndex)
 349 {
 350         unsigned char SRdata;
 351         int i;
 352
 353         SiS_SetReg(SiS_Pr, SiS_Pr->SiS_P3c4, 0x00, 0x03);
 354
 355         SRdata = SiS_Pr->SiS_StandTable[StandTableIndex].SR[0] | 0x20;
 356         SiS_SetReg(SiS_Pr, SiS_Pr->SiS_P3c4, 0x01, SRdata);
 357
 358         for (i = 2; i <= 4; i++) {
 359                 SRdata = SiS_Pr->SiS_StandTable[StandTableIndex].SR[i - 1];
 360                 SiS_SetReg(SiS_Pr, SiS_Pr->SiS_P3c4, i, SRdata);
 361         }
 362 }
 363
 364 /*********************************************/
 365 /*                  MISC                     */
 366 /*********************************************/
 367
 368 static void
 369 SiS_SetMiscRegs(struct SiS_Private *SiS_Pr, unsigned short StandTableIndex)
 370 {
 371         unsigned char Miscdata = SiS_Pr->SiS_StandTable[StandTableIndex].MISC;
 372
 373         SiS_SetRegByte(SiS_Pr, SiS_Pr->SiS_P3c2, Miscdata);
 374 }
 375
 376 /*********************************************/
 377 /*                  CRTC                     */
 378 /*********************************************/
 379
 380 static void
 381 SiS_SetCRTCRegs(struct SiS_Private *SiS_Pr, unsigned short StandTableIndex)
 382 {
 383         unsigned char CRTCdata;
 384         unsigned short i;
 385
 386         SiS_SetRegAND(SiS_Pr, SiS_Pr->SiS_P3d4, 0x11, 0x7f);
 387
 388         for (i = 0; i <= 0x18; i++) {
 389                 CRTCdata = SiS_Pr->SiS_StandTable[StandTableIndex].CRTC[i];
 390                 SiS_SetReg(SiS_Pr, SiS_Pr->SiS_P3d4, i, CRTCdata);
 391         }
 392 }
 393
 394 /*********************************************/
 395 /*                   ATT                     */
 396 /*********************************************/
 397
 398 static void
 399 SiS_SetATTRegs(struct SiS_Private *SiS_Pr, unsigned short StandTableIndex)
 400 {
 401         unsigned char ARdata;
 402         unsigned short i;
 403
 404         for (i = 0; i <= 0x13; i++) {
 405                 ARdata = SiS_Pr->SiS_StandTable[StandTableIndex].ATTR[i];
 406                 SiS_GetRegByte(SiS_Pr, SiS_Pr->SiS_P3da);
 407                 SiS_SetRegByte(SiS_Pr, SiS_Pr->SiS_P3c0, i);
 408                 SiS_SetRegByte(SiS_Pr, SiS_Pr->SiS_P3c0, ARdata);
 409         }
 410         SiS_GetRegByte(SiS_Pr, SiS_Pr->SiS_P3da);
 411         SiS_SetRegByte(SiS_Pr, SiS_Pr->SiS_P3c0, 0x14);
 412         SiS_SetRegByte(SiS_Pr, SiS_Pr->SiS_P3c0, 0x00);
 413
 414         SiS_GetRegByte(SiS_Pr, SiS_Pr->SiS_P3da);
 415         SiS_SetRegByte(SiS_Pr, SiS_Pr->SiS_P3c0, 0x20);
 416         SiS_GetRegByte(SiS_Pr, SiS_Pr->SiS_P3da);
 417 }
 418
 419 /*********************************************/
 420 /*                   GRC                     */
 421 /*********************************************/
 422
 423 static void
 424 SiS_SetGRCRegs(struct SiS_Private *SiS_Pr, unsigned short StandTableIndex)
 425 {
 426         unsigned char GRdata;
 427         unsigned short i;
 428
 429         for (i = 0; i <= 0x08; i++) {
 430                 GRdata = SiS_Pr->SiS_StandTable[StandTableIndex].GRC[i];
 431                 SiS_SetReg(SiS_Pr, SiS_Pr->SiS_P3ce, i, GRdata);
 432         }
 433
 434         if (SiS_Pr->SiS_ModeType > ModeVGA) {
 435                 /* 256 color disable */
 436                 SiS_SetRegAND(SiS_Pr, SiS_Pr->SiS_P3ce, 0x05, 0xBF);
 437         }
 438 }
 439
 440 /*********************************************/
 441 /*          CLEAR EXTENDED REGISTERS         */
 442 /*********************************************/
 443
 444 static void SiS_ClearExt1Regs(struct SiS_Private *SiS_Pr, unsigned short ModeNo)
 445 {
 446         int i;
 447
 448         for (i = 0x0A; i <= 0x0E; i++) {
 449                 SiS_SetReg(SiS_Pr, SiS_Pr->SiS_P3c4, i, 0x00);
 450         }
 451
 452         SiS_SetRegAND(SiS_Pr, SiS_Pr->SiS_P3c4, 0x37, 0xFE);
 453 }
 454
 455 /*********************************************/
 456 /*              Get rate index               */
 457 /*********************************************/
 458
 459 static unsigned short
 460 SiS_GetRatePtr(struct SiS_Private *SiS_Pr, unsigned short ModeNo,
 461                unsigned short ModeIdIndex)
 462 {
 463         unsigned short rrti, i, index, temp;
 464
 465         if (ModeNo <= 0x13)
 466                 return 0xFFFF;
 467
 468         index = SiS_GetReg(SiS_Pr, SiS_Pr->SiS_P3d4, 0x33) & 0x0F;
 469         if (index > 0)
 470                 index--;
 471
 472         rrti = SiS_Pr->SiS_EModeIDTable[ModeIdIndex].REFindex;
 473         ModeNo = SiS_Pr->SiS_RefIndex[rrti].ModeID;
 474
 475         i = 0;
 476         do {
 477                 if (SiS_Pr->SiS_RefIndex[rrti + i].ModeID != ModeNo)
 478                         break;
 479
 480                 temp =
 481                     SiS_Pr->SiS_RefIndex[rrti + i].Ext_InfoFlag & ModeTypeMask;
 482                 if (temp < SiS_Pr->SiS_ModeType)
 483                         break;
 484
 485                 i++;
 486                 index--;
 487         } while (index != 0xFFFF);
 488
 489         i--;
 490
 491         return (rrti + i);
 492 }
 493
 494 /*********************************************/
 495 /*                  SYNC                     */
 496 /*********************************************/
 497
 498 static void SiS_SetCRT1Sync(struct SiS_Private *SiS_Pr, unsigned short rrti)
 499 {
 500         unsigned short sync = SiS_Pr->SiS_RefIndex[rrti].Ext_InfoFlag >> 8;
 501         sync &= 0xC0;
 502         sync |= 0x2f;
 503         SiS_SetRegByte(SiS_Pr, SiS_Pr->SiS_P3c2, sync);
 504 }
 505
 506 /*********************************************/
 507 /*                  CRTC/2                   */
 508 /*********************************************/
 509
 510 static void
 511 SiS_SetCRT1CRTC(struct SiS_Private *SiS_Pr, unsigned short ModeNo,
 512                 unsigned short ModeIdIndex, unsigned short rrti)
 513 {
 514         unsigned char index;
 515         unsigned short temp, i, j, modeflag;
 516
 517         SiS_SetRegAND(SiS_Pr, SiS_Pr->SiS_P3d4, 0x11, 0x7f);
 518
 519         modeflag = SiS_Pr->SiS_EModeIDTable[ModeIdIndex].Ext_ModeFlag;
 520
 521         index = SiS_Pr->SiS_RefIndex[rrti].Ext_CRT1CRTC;
 522
 523         for (i = 0, j = 0; i <= 7; i++, j++) {
 524                 SiS_SetReg(SiS_Pr, SiS_Pr->SiS_P3d4, j,
 525                            SiS_Pr->SiS_CRT1Table[index].CR[i]);
 526         }
 527         for (j = 0x10; i <= 10; i++, j++) {
 528                 SiS_SetReg(SiS_Pr, SiS_Pr->SiS_P3d4, j,
 529                            SiS_Pr->SiS_CRT1Table[index].CR[i]);
 530         }
 531         for (j = 0x15; i <= 12; i++, j++) {
 532                 SiS_SetReg(SiS_Pr, SiS_Pr->SiS_P3d4, j,
 533                            SiS_Pr->SiS_CRT1Table[index].CR[i]);
 534         }
 535         for (j = 0x0A; i <= 15; i++, j++) {
 536                 SiS_SetReg(SiS_Pr, SiS_Pr->SiS_P3c4, j,
 537                            SiS_Pr->SiS_CRT1Table[index].CR[i]);
 538         }
 539
 540         temp = SiS_Pr->SiS_CRT1Table[index].CR[16] & 0xE0;
 541         SiS_SetReg(SiS_Pr, SiS_Pr->SiS_P3c4, 0x0E, temp);
 542
 543         temp = ((SiS_Pr->SiS_CRT1Table[index].CR[16]) & 0x01) << 5;
 544         if (modeflag & DoubleScanMode)
 545                 temp |= 0x80;
 546         SiS_SetRegANDOR(SiS_Pr, SiS_Pr->SiS_P3d4, 0x09, 0x5F, temp);
 547
 548         if (SiS_Pr->SiS_ModeType > ModeVGA)
 549                 SiS_SetReg(SiS_Pr, SiS_Pr->SiS_P3d4, 0x14, 0x4F);
 550 }
 551
 552 /*********************************************/
 553 /*               OFFSET & PITCH              */
 554 /*********************************************/
 555 /*  (partly overruled by SetPitch() in XF86) */
 556 /*********************************************/
 557
 558 static void
 559 SiS_SetCRT1Offset(struct SiS_Private *SiS_Pr, unsigned short ModeNo,
 560                   unsigned short ModeIdIndex, unsigned short rrti)
 561 {
 562         unsigned short du = SiS_GetOffset(SiS_Pr, ModeNo, ModeIdIndex, rrti);
 563         unsigned short infoflag = SiS_Pr->SiS_RefIndex[rrti].Ext_InfoFlag;
 564         unsigned short temp;
 565
 566         temp = (du >> 8) & 0x0f;
 567         SiS_SetRegANDOR(SiS_Pr, SiS_Pr->SiS_P3c4, 0x0E, 0xF0, temp);
 568
 569         SiS_SetReg(SiS_Pr, SiS_Pr->SiS_P3d4, 0x13, (du & 0xFF));
 570
 571         if (infoflag & InterlaceMode)
 572                 du >>= 1;
 573
 574         du <<= 5;
 575         temp = (du >> 8) & 0xff;
 576         if (du & 0xff)
 577                 temp++;
 578         temp++;
 579         SiS_SetReg(SiS_Pr, SiS_Pr->SiS_P3c4, 0x10, temp);
 580 }
 581
 582 /*********************************************/
 583 /*                  VCLK                     */
 584 /*********************************************/
 585
 586 static void
 587 SiS_SetCRT1VCLK(struct SiS_Private *SiS_Pr, unsigned short ModeNo,
 588                 unsigned short rrti)
 589 {
 590         unsigned short index = SiS_Pr->SiS_RefIndex[rrti].Ext_CRTVCLK;
 591         unsigned short clka = SiS_Pr->SiS_VCLKData[index].SR2B;
 592         unsigned short clkb = SiS_Pr->SiS_VCLKData[index].SR2C;
 593
 594         SiS_SetRegAND(SiS_Pr, SiS_Pr->SiS_P3c4, 0x31, 0xCF);
 595
 596         SiS_SetReg(SiS_Pr, SiS_Pr->SiS_P3c4, 0x2B, clka);
 597         SiS_SetReg(SiS_Pr, SiS_Pr->SiS_P3c4, 0x2C, clkb);
 598         SiS_SetReg(SiS_Pr, SiS_Pr->SiS_P3c4, 0x2D, 0x01);
 599 }
 600
 601 /*********************************************/
 602 /*                  FIFO                     */
 603 /*********************************************/
 604
 605 static void
 606 SiS_SetCRT1FIFO_310(struct SiS_Private *SiS_Pr, unsigned short ModeNo,
 607                     unsigned short mi)
 608 {
 609         unsigned short modeflag = SiS_Pr->SiS_EModeIDTable[mi].Ext_ModeFlag;
 610
 611         /* disable auto-threshold */
 612         SiS_SetRegAND(SiS_Pr, SiS_Pr->SiS_P3c4, 0x3D, 0xFE);
 613
 614         SiS_SetReg(SiS_Pr, SiS_Pr->SiS_P3c4, 0x08, 0xAE);
 615         SiS_SetRegAND(SiS_Pr, SiS_Pr->SiS_P3c4, 0x09, 0xF0);
 616
 617         if (ModeNo <= 0x13)
 618                 return;
 619
 620         if ((!(modeflag & DoubleScanMode)) || (!(modeflag & HalfDCLK))) {
 621                 SiS_SetReg(SiS_Pr, SiS_Pr->SiS_P3c4, 0x08, 0x34);
 622                 SiS_SetRegOR(SiS_Pr, SiS_Pr->SiS_P3c4, 0x3D, 0x01);
 623         }
 624 }
 625
 626 /*********************************************/
 627 /*              MODE REGISTERS               */
 628 /*********************************************/
 629
 630 static void
 631 SiS_SetVCLKState(struct SiS_Private *SiS_Pr, unsigned short ModeNo,
 632                  unsigned short rrti)
 633 {
 634         unsigned short data = 0, VCLK = 0, index = 0;
 635
 636         if (ModeNo > 0x13) {
 637                 index = SiS_Pr->SiS_RefIndex[rrti].Ext_CRTVCLK;
 638                 VCLK = SiS_Pr->SiS_VCLKData[index].CLOCK;
 639         }
 640
 641         if (VCLK >= 166)
 642                 data |= 0x0c;
 643         SiS_SetRegANDOR(SiS_Pr, SiS_Pr->SiS_P3c4, 0x32, 0xf3, data);
 644
 645         if (VCLK >= 166)
 646                 SiS_SetRegAND(SiS_Pr, SiS_Pr->SiS_P3c4, 0x1f, 0xe7);
 647
 648         /* DAC speed */
 649         data = 0x03;
 650         if (VCLK >= 260)
 651                 data = 0x00;
 652         else if (VCLK >= 160)
 653                 data = 0x01;
 654         else if (VCLK >= 135)
 655                 data = 0x02;
 656
 657         SiS_SetRegANDOR(SiS_Pr, SiS_Pr->SiS_P3c4, 0x07, 0xF8, data);
 658 }
 659
 660 static void
 661 SiS_SetCRT1ModeRegs(struct SiS_Private *SiS_Pr, unsigned short ModeNo,
 662                     unsigned short ModeIdIndex, unsigned short rrti)
 663 {
 664         unsigned short data, infoflag = 0, modeflag;
 665
 666         if (ModeNo <= 0x13)
 667                 modeflag = SiS_Pr->SiS_SModeIDTable[ModeIdIndex].St_ModeFlag;
 668         else {
 669                 modeflag = SiS_Pr->SiS_EModeIDTable[ModeIdIndex].Ext_ModeFlag;
 670                 infoflag = SiS_Pr->SiS_RefIndex[rrti].Ext_InfoFlag;
 671         }
 672
 673         /* Disable DPMS */
 674         SiS_SetRegAND(SiS_Pr, SiS_Pr->SiS_P3c4, 0x1F, 0x3F);
 675
 676         data = 0;
 677         if (ModeNo > 0x13) {
 678                 if (SiS_Pr->SiS_ModeType > ModeEGA) {
 679                         data |= 0x02;
 680                         data |= ((SiS_Pr->SiS_ModeType - ModeVGA) << 2);
 681                 }
 682                 if (infoflag & InterlaceMode)
 683                         data |= 0x20;
 684         }
 685         SiS_SetRegANDOR(SiS_Pr, SiS_Pr->SiS_P3c4, 0x06, 0xC0, data);
 686
 687         data = 0;
 688         if (infoflag & InterlaceMode) {
 689                 /* data = (Hsync / 8) - ((Htotal / 8) / 2) + 3 */
 690                 unsigned short hrs =
 691                     (SiS_GetReg(SiS_Pr, SiS_Pr->SiS_P3d4, 0x04) |
 692                      ((SiS_GetReg(SiS_Pr, SiS_Pr->SiS_P3c4, 0x0b) & 0xc0) << 2))
 693                     - 3;
 694                 unsigned short hto =
 695                     (SiS_GetReg(SiS_Pr, SiS_Pr->SiS_P3d4, 0x00) |
 696                      ((SiS_GetReg(SiS_Pr, SiS_Pr->SiS_P3c4, 0x0b) & 0x03) << 8))
 697                     + 5;
 698                 data = hrs - (hto >> 1) + 3;
 699         }
 700         SiS_SetReg(SiS_Pr, SiS_Pr->SiS_P3d4, 0x19, (data & 0xFF));
 701         SiS_SetRegANDOR(SiS_Pr, SiS_Pr->SiS_P3d4, 0x1a, 0xFC, (data >> 8));
 702
 703         if (modeflag & HalfDCLK)
 704                 SiS_SetRegOR(SiS_Pr, SiS_Pr->SiS_P3c4, 0x01, 0x08);
 705
 706         data = 0;
 707         if (modeflag & LineCompareOff)
 708                 data = 0x08;
 709         SiS_SetRegANDOR(SiS_Pr, SiS_Pr->SiS_P3c4, 0x0F, 0xB7, data);
 710
 711         if ((SiS_Pr->SiS_ModeType == ModeEGA) && (ModeNo > 0x13))
 712                 SiS_SetRegOR(SiS_Pr, SiS_Pr->SiS_P3c4, 0x0F, 0x40);
 713
 714         SiS_SetRegAND(SiS_Pr, SiS_Pr->SiS_P3c4, 0x31, 0xfb);
 715
 716         data = 0x60;
 717         if (SiS_Pr->SiS_ModeType != ModeText) {
 718                 data ^= 0x60;
 719                 if (SiS_Pr->SiS_ModeType != ModeEGA)
 720                         data ^= 0xA0;
 721         }
 722         SiS_SetRegANDOR(SiS_Pr, SiS_Pr->SiS_P3c4, 0x21, 0x1F, data);
 723
 724         SiS_SetVCLKState(SiS_Pr, ModeNo, rrti);
 725
 726         if (SiS_GetReg(SiS_Pr, SiS_Pr->SiS_P3d4, 0x31) & 0x40)
 727                 SiS_SetReg(SiS_Pr, SiS_Pr->SiS_P3d4, 0x52, 0x2c);
 728         else
 729                 SiS_SetReg(SiS_Pr, SiS_Pr->SiS_P3d4, 0x52, 0x6c);
 730 }
 731
 732 /*********************************************/
 733 /*                 LOAD DAC                  */
 734 /*********************************************/
 735
 736 static void
 737 SiS_WriteDAC(struct SiS_Private *SiS_Pr, unsigned long DACData,
 738              unsigned short shiftflag, unsigned short dl, unsigned short ah,
 739              unsigned short al, unsigned short dh)
 740 {
 741         unsigned short d1, d2, d3;
 742
 743         switch (dl) {
 744         case 0:
 745                 d1 = dh;
 746                 d2 = ah;
 747                 d3 = al;
 748                 break;
 749         case 1:
 750                 d1 = ah;
 751                 d2 = al;
 752                 d3 = dh;
 753                 break;
 754         default:
 755                 d1 = al;
 756                 d2 = dh;
 757                 d3 = ah;
 758         }
 759         SiS_SetRegByte(SiS_Pr, DACData, (d1 << shiftflag));
 760         SiS_SetRegByte(SiS_Pr, DACData, (d2 << shiftflag));
 761         SiS_SetRegByte(SiS_Pr, DACData, (d3 << shiftflag));
 762 }
 763
 764 static void
 765 SiS_LoadDAC(struct SiS_Private *SiS_Pr, unsigned short ModeNo,
 766             unsigned short mi)
 767 {
 768         unsigned short data, data2, time, i, j, k, m, n, o;
 769         unsigned short si, di, bx, sf;
 770         unsigned long DACAddr, DACData;
 771         const unsigned char *table = NULL;
 772
 773         if (ModeNo < 0x13)
 774                 data = SiS_Pr->SiS_SModeIDTable[mi].St_ModeFlag;
 775         else
 776                 data = SiS_Pr->SiS_EModeIDTable[mi].Ext_ModeFlag;
 777
 778         data &= DACInfoFlag;
 779
 780         j = time = 64;
 781         if (data == 0x00)
 782                 table = SiS_MDA_DAC;
 783         else if (data == 0x08)
 784                 table = SiS_CGA_DAC;
 785         else if (data == 0x10)
 786                 table = SiS_EGA_DAC;
 787         else {
 788                 j = 16;
 789                 time = 256;
 790                 table = SiS_VGA_DAC;
 791         }
 792
 793         DACAddr = SiS_Pr->SiS_P3c8;
 794         DACData = SiS_Pr->SiS_P3c9;
 795         sf = 0;
 796         SiS_SetRegByte(SiS_Pr, SiS_Pr->SiS_P3c6, 0xFF);
 797
 798         SiS_SetRegByte(SiS_Pr, DACAddr, 0x00);
 799
 800         for (i = 0; i < j; i++) {
 801                 data = table[i];
 802                 for (k = 0; k < 3; k++) {
 803                         data2 = 0;
 804                         if (data & 0x01)
 805                                 data2 += 0x2A;
 806                         if (data & 0x02)
 807                                 data2 += 0x15;
 808                         SiS_SetRegByte(SiS_Pr, DACData, (data2 << sf));
 809                         data >>= 2;
 810                 }
 811         }
 812
 813         if (time == 256) {
 814                 for (i = 16; i < 32; i++) {
 815                         data = table[i] << sf;
 816                         for (k = 0; k < 3; k++)
 817                                 SiS_SetRegByte(SiS_Pr, DACData, data);
 818                 }
 819                 si = 32;
 820                 for (m = 0; m < 9; m++) {
 821                         di = si;
 822                         bx = si + 4;
 823                         for (n = 0; n < 3; n++) {
 824                                 for (o = 0; o < 5; o++) {
 825                                         SiS_WriteDAC(SiS_Pr, DACData, sf, n,
 826                                                      table[di], table[bx],
 827                                                      table[si]);
 828                                         si++;
 829                                 }
 830                                 si -= 2;
 831                                 for (o = 0; o < 3; o++) {
 832                                         SiS_WriteDAC(SiS_Pr, DACData, sf, n,
 833                                                      table[di], table[si],
 834                                                      table[bx]);
 835                                         si--;
 836                                 }
 837                         }
 838                         si += 5;
 839                 }
 840         }
 841 }
 842
 843 /*********************************************/
 844 /*         SET CRT1 REGISTER GROUP           */
 845 /*********************************************/
 846
 847 static void
 848 SiS_SetCRT1Group(struct SiS_Private *SiS_Pr, unsigned short ModeNo,
 849                  unsigned short ModeIdIndex)
 850 {
 851         unsigned short StandTableIndex, rrti;
 852
 853         SiS_Pr->SiS_CRT1Mode = ModeNo;
 854
 855         if (ModeNo <= 0x13)
 856                 StandTableIndex = 0;
 857         else
 858                 StandTableIndex = 1;
 859
 860         SiS_ResetSegmentRegisters(SiS_Pr);
 861         SiS_SetSeqRegs(SiS_Pr, StandTableIndex);
 862         SiS_SetMiscRegs(SiS_Pr, StandTableIndex);
 863         SiS_SetCRTCRegs(SiS_Pr, StandTableIndex);
 864         SiS_SetATTRegs(SiS_Pr, StandTableIndex);
 865         SiS_SetGRCRegs(SiS_Pr, StandTableIndex);
 866         SiS_ClearExt1Regs(SiS_Pr, ModeNo);
 867
 868         rrti = SiS_GetRatePtr(SiS_Pr, ModeNo, ModeIdIndex);
 869
 870         if (rrti != 0xFFFF) {
 871                 SiS_SetCRT1Sync(SiS_Pr, rrti);
 872                 SiS_SetCRT1CRTC(SiS_Pr, ModeNo, ModeIdIndex, rrti);
 873                 SiS_SetCRT1Offset(SiS_Pr, ModeNo, ModeIdIndex, rrti);
 874                 SiS_SetCRT1VCLK(SiS_Pr, ModeNo, rrti);
 875         }
 876
 877         SiS_SetCRT1FIFO_310(SiS_Pr, ModeNo, ModeIdIndex);
 878
 879         SiS_SetCRT1ModeRegs(SiS_Pr, ModeNo, ModeIdIndex, rrti);
 880
 881         SiS_LoadDAC(SiS_Pr, ModeNo, ModeIdIndex);
 882
 883         SiS_DisplayOn(SiS_Pr);
 884 }
 885
 886 /*********************************************/
 887 /*                 SiSSetMode()              */
 888 /*********************************************/
 889
 890 int SiSUSBSetMode(struct SiS_Private *SiS_Pr, unsigned short ModeNo)
 891 {
 892         unsigned short ModeIdIndex;
 893         unsigned long BaseAddr = SiS_Pr->IOAddress;
 894
 895         SiSUSB_InitPtr(SiS_Pr);
 896         SiSUSBRegInit(SiS_Pr, BaseAddr);
 897         SiS_GetSysFlags(SiS_Pr);
 898
 899         if (!(SiS_SearchModeID(SiS_Pr, &ModeNo, &ModeIdIndex)))
 900                 return 0;
 901
 902         SiS_SetReg(SiS_Pr, SiS_Pr->SiS_P3c4, 0x05, 0x86);
 903
 904         SiSInitPCIetc(SiS_Pr);
 905
 906         ModeNo &= 0x7f;
 907
 908         SiS_Pr->SiS_ModeType =
 909             SiS_Pr->SiS_EModeIDTable[ModeIdIndex].Ext_ModeFlag & ModeTypeMask;
 910
 911         SiS_Pr->SiS_SetFlag = LowModeTests;
 912
 913         /* Set mode on CRT1 */
 914         SiS_SetCRT1Group(SiS_Pr, ModeNo, ModeIdIndex);
 915
 916         SiS_HandleCRT1(SiS_Pr);
 917
 918         SiS_DisplayOn(SiS_Pr);
 919         SiS_SetRegByte(SiS_Pr, SiS_Pr->SiS_P3c6, 0xFF);
 920
 921         /* Store mode number */
 922         SiS_SetReg(SiS_Pr, SiS_Pr->SiS_P3d4, 0x34, ModeNo);
 923
 924         return 1;
 925 }
 926
 927 int SiSUSBSetVESAMode(struct SiS_Private *SiS_Pr, unsigned short VModeNo)
 928 {
 929         unsigned short ModeNo = 0;
 930         int i;
 931
 932         SiSUSB_InitPtr(SiS_Pr);
 933
 934         if (VModeNo == 0x03) {
 935
 936                 ModeNo = 0x03;
 937
 938         } else {
 939
 940                 i = 0;
 941                 do {
 942
 943                         if (SiS_Pr->SiS_EModeIDTable[i].Ext_VESAID == VModeNo) {
 944                                 ModeNo = SiS_Pr->SiS_EModeIDTable[i].Ext_ModeID;
 945                                 break;
 946                         }
 947
 948                 } while (SiS_Pr->SiS_EModeIDTable[i++].Ext_ModeID != 0xff);
 949
 950         }
 951
 952         if (!ModeNo)
 953                 return 0;
 954
 955         return SiSUSBSetMode(SiS_Pr, ModeNo);
 956 }
 957
 958 #endif /* INCL_SISUSB_CON */