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
 55static 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
 72static void
 73SiS_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
 79static void
 80SiS_SetRegByte(struct SiS_Private *SiS_Pr, unsigned long port,
 81	       unsigned short data)
 82{
 83	sisusb_setreg(SiS_Pr->sisusb, port, data);
 84}
 85
 86static unsigned char
 87SiS_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
 96static unsigned char
 97SiS_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
106static void
107SiS_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
114static void
115SiS_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
121static void
122SiS_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
132static 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
141static 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
164static 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
173static 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
189static 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
202static 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
215static 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
221static void SiS_ResetSegmentReg(struct SiS_Private *SiS_Pr)
222{
223	SiS_SetSegmentReg(SiS_Pr, 0);
224}
225
226static void
227SiS_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
237static void SiS_ResetSegmentRegOver(struct SiS_Private *SiS_Pr)
238{
239	SiS_SetSegmentRegOver(SiS_Pr, 0);
240}
241
242static 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
252static int
253SiS_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
285static 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
295static unsigned short
296SiS_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
319static unsigned short
320SiS_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
347static void
348SiS_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
368static void
369SiS_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
380static void
381SiS_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
398static void
399SiS_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
423static void
424SiS_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
444static 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
459static unsigned short
460SiS_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
498static 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
510static void
511SiS_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
558static void
559SiS_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
586static void
587SiS_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
605static void
606SiS_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
630static void
631SiS_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
660static void
661SiS_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
736static void
737SiS_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
764static void
765SiS_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
847static void
848SiS_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
890int 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
927int 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 */