Split SMI501_ModeInit in two functions.

  This allows using alternate MSOCRegRec structures to set a mode.
This should be useful to restore the initial mode.

2 files changed, 148 insertions, 130 deletions

diff --git a/src/smi_501.c b/src/smi_501.c
index 1cd765c..f7cfbf9 100644
--- a/src/smi_501.c
+++ b/src/smi_501.c
@@ -49,10 +49,11 @@ authorization from The XFree86 Project or Silicon Motion.
 #undef VERBLEV
 #define VERBLEV		1
 
+static void SMI501_ModeSet(ScrnInfoPtr pScrn, MSOCRegPtr mode);
 static char *format_integer_base2(int32_t word);
 static void SMI501_PrintRegs(ScrnInfoPtr pScrn);
 static void SMI501_SetClock(SMIPtr pSmi, int32_t port,
-			    int32_t clock, int32_t value);
+			    int32_t pll, int32_t value);
 static void SMI501_WaitVSync(SMIPtr pSmi, int vsync_count);
 
 Bool
@@ -229,7 +230,6 @@ SMI501_ModeInit(ScrnInfoPtr pScrn, DisplayModePtr xf86mode)
     MSOCRegPtr	mode;
     SMIPtr	pSmi = SMIPTR(pScrn);
     double	mclk;
-    int32_t	clock;
     int		diff, best, divider, shift, x2_divider, x2_shift;
 
     save = pSmi->save;
@@ -262,48 +262,38 @@ SMI501_ModeInit(ScrnInfoPtr pScrn, DisplayModePtr xf86mode)
     /* Gpio, Pwm, and I2c */
     field(mode->gate, gpio) = 1;
 
-    /* Update gate first */
-    WRITE_SCR(pSmi, mode->current_gate, mode->gate.value);
-
     /* FIXME fixed at power mode 0 as in the smi sources */
     field(mode->power_ctl, status) = 0;
     field(mode->power_ctl, mode) = 0;
 
     /* FIXME fixed at 336/3/0 as in the smi sources */
     field(mode->clock, m_select) = 1;
-    clock = mode->clock.value;
     field(mode->clock, m_divider) = 1;
     field(mode->clock, m_shift) = 0;
-    SMI501_SetClock(pSmi, mode->current_clock, clock, mode->clock.value);
 
     switch (pSmi->MCLK) {
 	case 168000:	    /* 336/1/1 */
 	    field(mode->clock, m2_select) = 1;
-	    clock = mode->clock.value;
 	    field(mode->clock, m2_divider) = 0;
 	    field(mode->clock, m2_shift) = 1;
 	    break;
 	case 96000:	    /* 288/3/0 */
 	    field(mode->clock, m2_select) = 0;
-	    clock = mode->clock.value;
 	    field(mode->clock, m2_divider) = 1;
 	    field(mode->clock, m2_shift) = 0;
 	    break;
 	case 144000:	    /* 288/1/1 */
 	    field(mode->clock, m2_select) = 0;
-	    clock = mode->clock.value;
 	    field(mode->clock, m2_divider) = 0;
 	    field(mode->clock, m2_shift) = 1;
 	    break;
 	case 112000:	    /* 336/3/0 */
 	default:
 	    field(mode->clock, m2_select) = 1;
-	    clock = mode->clock.value;
 	    field(mode->clock, m2_divider) = 1;
 	    field(mode->clock, m2_shift) = 0;
 	    break;
     }
-    SMI501_SetClock(pSmi, mode->current_clock, clock, mode->clock.value);
 
     /* Find clock best matching mode */
     best = 0x7fffffff;
@@ -329,10 +319,8 @@ SMI501_ModeInit(ScrnInfoPtr pScrn, DisplayModePtr xf86mode)
 
     if (pSmi->lcd) {
 	field(mode->clock, p2_select) = mclk == 288000.0 ? 0 : 1;
-	clock = mode->clock.value;
 	field(mode->clock, p2_divider) = x2_divider;
 	field(mode->clock, p2_shift) = x2_shift;
-	SMI501_SetClock(pSmi, mode->current_clock, clock, mode->clock.value);
 
 	field(mode->panel_display_ctl, format) =
 	    pScrn->bitsPerPixel == 8 ? 0 :
@@ -384,10 +372,8 @@ SMI501_ModeInit(ScrnInfoPtr pScrn, DisplayModePtr xf86mode)
     }
     else {
 	field(mode->clock, v2_select) = mclk == 288000.0 ? 0 : 1;
-	clock = mode->clock.value;
 	field(mode->clock, v2_divider) = x2_divider;
 	field(mode->clock, v2_shift) = x2_shift;
-	SMI501_SetClock(pSmi, mode->current_clock, clock, mode->clock.value);
 
 	field(mode->crt_display_ctl, format) =
 	    pScrn->bitsPerPixel == 8 ? 0 :
@@ -427,6 +413,41 @@ SMI501_ModeInit(ScrnInfoPtr pScrn, DisplayModePtr xf86mode)
 	    xf86mode->HSyncStart;
     }
 
+    SMI501_ModeSet(pScrn, mode);
+
+    SMI_AdjustFrame(pScrn->scrnIndex, pScrn->frameX0, pScrn->frameY0, 0);
+
+    xf86DrvMsgVerb(pScrn->scrnIndex, X_INFO, VERBLEV,
+		   "Register dump (After Mode Init)\n");
+    SMI501_PrintRegs(pScrn);
+
+    return (TRUE);
+}
+
+static void
+SMI501_ModeSet(ScrnInfoPtr pScrn, MSOCRegPtr mode)
+{
+    MSOCClockRec	clock;
+    SMIPtr		pSmi = SMIPTR(pScrn);
+
+    /* Update gate first */
+    WRITE_SCR(pSmi, mode->current_gate, mode->gate.value);
+
+    clock.value = READ_SCR(pSmi, mode->current_clock);
+    field(clock, m_select) = field(mode->clock, m_select);
+    SMI501_SetClock(pSmi, mode->current_clock, clock.value, mode->clock.value);
+
+    clock.value = READ_SCR(pSmi, mode->current_clock);
+    field(clock, m2_select) = field(mode->clock, m2_select);
+    SMI501_SetClock(pSmi, mode->current_clock, clock.value, mode->clock.value);
+
+    clock.value = READ_SCR(pSmi, mode->current_clock);
+    if (pSmi->lcd)
+	field(clock, p2_select) = field(mode->clock, p2_select);
+    else
+	field(clock, v2_select) = field(mode->clock, v2_select);
+    SMI501_SetClock(pSmi, mode->current_clock, clock.value, mode->clock.value);
+
     WRITE_SCR(pSmi, MISC_CTL, mode->misc_ctl.value);
 
     if (pSmi->lcd) {
@@ -485,19 +506,14 @@ SMI501_ModeInit(ScrnInfoPtr pScrn, DisplayModePtr xf86mode)
 
     WRITE_SCR(pSmi, POWER_CTL, mode->power_ctl.value);
 
-    if (pSmi->PCIBurst) {
-	field(mode->system_ctl, burst) = field(mode->system_ctl, burst_read) = 1;
-	field(mode->system_ctl, retry) = pSmi->PCIRetry != FALSE;
-	WRITE_SCR(pSmi, SYSTEM_CTL, mode->system_ctl.value);
-    }
-
-    SMI_AdjustFrame(pScrn->scrnIndex, pScrn->frameX0, pScrn->frameY0, 0);
-
-    xf86DrvMsgVerb(pScrn->scrnIndex, X_INFO, VERBLEV,
-		   "Register dump (After Mode Init)\n");
-    SMI501_PrintRegs(pScrn);
-
-    return (TRUE);
+    /* Match configuration */
+    /* FIXME some other fields should also be set, otherwise, since
+     * neither kernel nor driver change it, a reboot is required to
+     * modify or reset to default */
+    field(mode->system_ctl, burst) = field(mode->system_ctl, burst_read) =
+	pSmi->PCIBurst != FALSE;
+    field(mode->system_ctl, retry) = pSmi->PCIRetry != FALSE;
+    WRITE_SCR(pSmi, SYSTEM_CTL, mode->system_ctl.value);
 }
 
 void
@@ -573,7 +589,7 @@ SMI501_WaitVSync(SMIPtr pSmi, int vsync_count)
 }
 
 static void
-SMI501_SetClock(SMIPtr pSmi, int32_t port, int32_t clock, int32_t value)
+SMI501_SetClock(SMIPtr pSmi, int32_t port, int32_t pll, int32_t value)
 {
     /*
      *	Rules to Program the Power Mode Clock Registers for Clock Selection
@@ -597,10 +613,10 @@ SMI501_SetClock(SMIPtr pSmi, int32_t port, int32_t clock, int32_t value)
      */
 
     /* register contents selecting clock */
-    WRITE_SCR(pSmi, port, clock);
+    WRITE_SCR(pSmi, port, pll);
     SMI501_WaitVSync(pSmi, 1);
 
     /* full register contents */
-    WRITE_SCR(pSmi, port, clock);
+    WRITE_SCR(pSmi, port, pll);
     SMI501_WaitVSync(pSmi, 1);
 }
diff --git a/src/smi_501.h b/src/smi_501.h
index 8e1719d..92cea0b 100644
--- a/src/smi_501.h
+++ b/src/smi_501.h
@@ -46,6 +46,104 @@ authorization from the XFree86 Project and Silicon Motion.
 /* contents of either power0_clock or power1_clock */
 #define CURRENT_CLOCK			0x00003c
 
+#define POWER0_CLOCK			0x000044
+#define POWER1_CLOCK			0x00004c
+/*  POWER MODE 0 CLOCK
+ *  Read/Write MMIO_base + 0x000044
+ *  Power-on Default 0x2A1A0A09
+ *
+ *  POWER MODE 1 CLOCK
+ *  Read/Write MMIO_base + 0x00004C
+ *  Power-on Default 0x2A1A0A09
+ *
+ *  0:3     M2XCLK Frequency Divider
+ *	    0000 / 1	    1000 / 3
+ *	    0001 / 2	    1001 / 6
+ *	    0010 / 4	    1010 / 12
+ *	    0011 / 8	    1011 / 24
+ *	    0100 / 16	    1100 / 48
+ *	    0101 / 32	    1101 / 96
+ *	    0110 / 64	    1110 / 192
+ *	    0111 / 128	    1111 / 384
+ *  4:4     M2XCLK Frequency Input Select.
+ *	    0: 288 MHz.
+ *	    1: 336 MHz/288 MHz/240 MHz/192 MHz
+ *	       (see bits 5:4 in the Miscellaneous Timing register
+ *		at offset 0x68 on page 2-42).
+ *  8:11    MCLK Frequency Divider.
+ *	    0000 / 1	    1000 / 3
+ *	    0001 / 2	    1001 / 6
+ *	    0010 / 4	    1010 / 12
+ *	    0011 / 8	    1011 / 24
+ *	    0100 / 16	    1100 / 48
+ *	    0101 / 32	    1101 / 96
+ *	    0110 / 64	    1110 / 192
+ *	    0111 / 128	    1111 / 384
+ *  12:12   MCLK Frequency Input Select.
+ *	    0: 288 MHz.
+ *	    1: 336 MHz/288 MHz/240 MHz/192 MHz
+ *	       (see bits 5:4 in the Miscellaneous Timing register
+ *		at offset 0x68 on page 2-42).
+ * 16:19    V2XCLK DIVIDER
+ *	    0000 / 1	    1000 / 3
+ *	    0001 / 2	    1001 / 6
+ *	    0010 / 4	    1010 / 12
+ *	    0011 / 8	    1011 / 24
+ *	    0100 / 16	    1100 / 48
+ *	    0101 / 32	    1101 / 96
+ *	    0110 / 64	    1110 / 192
+ *	    0111 / 128	    1111 / 384
+ * 20:20    V2XCLK SELECT (Crt clock)
+ *	    0: 288 MHz
+ *	    1: 336 MHz/288 MHz/240 MHz/192 MHz
+ *	       (see bits 5:4 in the Miscellaneous Timing register
+ *		at offset 0x68 on page 2-42).
+ * 24:28    P2XCLK DIVIDER
+ *	    00000 / 1	    01000 / 3	    10000 / 5
+ *	    00001 / 2	    01001 / 6	    10001 / 10
+ *	    00010 / 4	    01010 / 12	    10010 / 20
+ *	    00011 / 8	    01011 / 24	    10011 / 40
+ *	    00100 / 16	    01100 / 48	    10100 / 80
+ *	    00101 / 32	    01101 / 96	    10101 / 160
+ *	    00110 / 64	    01110 / 192     10110 / 320
+ *	    00111 / 128     01111 / 384     10111 / 640
+ *  29:29   P2XCLK SELECT (Panel clock)
+ *	    0: 288 MHz
+ *	    1: 336 MHz/288 MHz/240 MHz/192 MHz
+ *	      (see bits 5:4 in the Miscellaneous Timing register
+ *	       at offset 0x68 on page 2-42).
+ *
+ * Remarks:
+ *  Table 2-2: Programmable Clock Branches
+ *  Clock Description
+ *  P2XCLK  2X clock source for the Panel interface timing.
+ *	    The actual rate at which the pixels are shifted
+ *	    out is P2XCLK divided by two.
+ *  V2XCLK  2X clock source for the CRT interface timing.
+ *	    The actual rate at which the pixels are shifted
+ *	    out is V2XCLK divided by two
+ */
+typedef union _MSOCClockRec {
+    struct {
+	int32_t 	m2_shift	: bitfield( 0,  2);
+	int32_t 	m2_divider	: bitfield( 3,  3);
+	int32_t		m2_select	: bitfield( 4,  4);
+	int32_t 	u0		: bitfield( 5,  7);
+	int32_t 	m_shift		: bitfield( 8, 10);
+	int32_t 	m_divider	: bitfield(11, 11);
+	int32_t 	m_select	: bitfield(12, 12);
+	int32_t 	u1		: bitfield(13, 15);
+	int32_t 	v2_shift	: bitfield(16, 18);
+	int32_t 	v2_divider	: bitfield(19, 19);
+	int32_t 	v2_select	: bitfield(20, 20);
+	int32_t 	u2		: bitfield(21, 23);
+	int32_t 	p2_shift	: bitfield(24, 26);
+	int32_t 	p2_divider	: bitfield(27, 28);
+	int32_t 	p2_select	: bitfield(29, 29);
+    } detail;
+    int32_t		value;
+} MSOCClockRec, *MSOCClockPtr;
+
 typedef struct _MSOCRegRec {
 #define SYSTEM_CTL			0x000000
     /*	SYSTEM CONTROL
@@ -135,104 +233,8 @@ typedef struct _MSOCRegRec {
     } gate;
     int32_t	current_gate;
 
-#define POWER0_CLOCK			0x000044
-#define POWER1_CLOCK			0x00004c
-    /*	POWER MODE 0 CLOCK
-     *	Read/Write MMIO_base + 0x000044
-     *	Power-on Default 0x2A1A0A09
-     *
-     *	POWER MODE 1 CLOCK
-     *	Read/Write MMIO_base + 0x00004C
-     *	Power-on Default 0x2A1A0A09
-     *
-     *	0:3	M2XCLK Frequency Divider
-     *		0000 / 1	1000 / 3
-     *		0001 / 2	1001 / 6
-     *		0010 / 4	1010 / 12
-     *		0011 / 8	1011 / 24
-     *		0100 / 16	1100 / 48
-     *		0101 / 32	1101 / 96
-     *		0110 / 64	1110 / 192
-     *		0111 / 128	1111 / 384
-     *	4:4	M2XCLK Frequency Input Select.
-     *		0: 288 MHz.
-     *		1: 336 MHz/288 MHz/240 MHz/192 MHz
-     *		   (see bits 5:4 in the Miscellaneous Timing register
-     *		    at offset 0x68 on page 2-42).
-     *	8:11	MCLK Frequency Divider.
-     *		0000 / 1	1000 / 3
-     *		0001 / 2	1001 / 6
-     *		0010 / 4	1010 / 12
-     *		0011 / 8	1011 / 24
-     *		0100 / 16	1100 / 48
-     *		0101 / 32	1101 / 96
-     *		0110 / 64	1110 / 192
-     *		0111 / 128	1111 / 384
-     *	12:12	MCLK Frequency Input Select.
-     *		0: 288 MHz.
-     *		1: 336 MHz/288 MHz/240 MHz/192 MHz
-     *		   (see bits 5:4 in the Miscellaneous Timing register
-     *		    at offset 0x68 on page 2-42).
-     * 16:19	V2XCLK DIVIDER
-     *		0000 / 1	1000 / 3
-     *		0001 / 2	1001 / 6
-     *		0010 / 4	1010 / 12
-     *		0011 / 8	1011 / 24
-     *		0100 / 16	1100 / 48
-     *		0101 / 32	1101 / 96
-     *		0110 / 64	1110 / 192
-     *		0111 / 128	1111 / 384
-     * 20:20	V2XCLK SELECT (Crt clock)
-     *		0: 288 MHz
-     *		1: 336 MHz/288 MHz/240 MHz/192 MHz
-     *		   (see bits 5:4 in the Miscellaneous Timing register
-     *		    at offset 0x68 on page 2-42).
-     * 24:28	P2XCLK DIVIDER
-     *		00000 / 1	01000 / 3	10000 / 5
-     *		00001 / 2	01001 / 6	10001 / 10
-     *		00010 / 4	01010 / 12	10010 / 20
-     *		00011 / 8	01011 / 24	10011 / 40
-     *		00100 / 16	01100 / 48	10100 / 80
-     *		00101 / 32	01101 / 96	10101 / 160
-     *		00110 / 64	01110 / 192     10110 / 320
-     *		00111 / 128	01111 / 384     10111 / 640
-     *	29:29	P2XCLK SELECT (Panel clock)
-     *		0: 288 MHz
-     *		1: 336 MHz/288 MHz/240 MHz/192 MHz
-     *		  (see bits 5:4 in the Miscellaneous Timing register
-     *		   at offset 0x68 on page 2-42).
-     *
-     * Remarks:
-     *	Table 2-2: Programmable Clock Branches
-     *	Clock Description
-     *	P2XCLK	2X clock source for the Panel interface timing.
-     *		The actual rate at which the pixels are shifted
-     *		out is P2XCLK divided by two.
-     *	V2XCLK	2X clock source for the CRT interface timing.
-     *		The actual rate at which the pixels are shifted
-     *		out is V2XCLK divided by two
-     */
-    union {
-	struct {
-	    int32_t	m2_shift	: bitfield( 0,  2);
-	    int32_t	m2_divider	: bitfield( 3,  3);
-	    int32_t	m2_select	: bitfield( 4,  4);
-	    int32_t	u0		: bitfield( 5,  7);
-	    int32_t	m_shift		: bitfield( 8, 10);
-	    int32_t	m_divider	: bitfield(11, 11);
-	    int32_t	m_select	: bitfield(12, 12);
-	    int32_t	u1		: bitfield(13, 15);
-	    int32_t	v2_shift	: bitfield(16, 18);
-	    int32_t	v2_divider	: bitfield(19, 19);
-	    int32_t	v2_select	: bitfield(20, 20);
-	    int32_t	u2		: bitfield(21, 23);
-	    int32_t	p2_shift	: bitfield(24, 26);
-	    int32_t	p2_divider	: bitfield(27, 28);
-	    int32_t	p2_select	: bitfield(29, 29);
-	} detail;
-	int32_t     value;
-    } clock;
-    int32_t	current_clock;
+    MSOCClockRec	clock;
+    int32_t		current_clock;
 
 #define SLEEP_GATE			0x000050
     /*	SLEEP MODE GATE