drivers/usb/misc/sisusbvga/sisusb_init.c

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