Add a function to lock/unlock all crtcs, use it in nv_bios code

Also split initial head owner determination into separate function

4 files changed, 66 insertions, 56 deletions

diff --git a/src/nv_bios.c b/src/nv_bios.c
index 76fd6d0..dd9aa0e 100644
--- a/src/nv_bios.c
+++ b/src/nv_bios.c
@@ -441,20 +441,6 @@ static void nv_port_wr(ScrnInfoPtr pScrn, uint16_t port, uint8_t data)
 	}
 }
 
-#define ACCESS_UNLOCK 0
-#define ACCESS_LOCK 1
-static void crtc_access(NVPtr pNv, bool lock)
-{
-	if (pNv->twoHeads)
-		NVSetOwner(pNv, 0);
-	NVLockVgaCrtc(pNv, 0, lock);
-	if (pNv->twoHeads) {
-		NVSetOwner(pNv, 1);
-		NVLockVgaCrtc(pNv, 1, lock);
-		NVSetOwner(pNv, crtchead);
-	}
-}
-
 static bool io_flag_condition(ScrnInfoPtr pScrn, bios_t *bios, uint16_t offset, uint8_t cond)
 {
 	/* The IO flag condition entry has 2 bytes for the CRTC port; 1 byte
@@ -4471,7 +4457,9 @@ bool NVRunVBIOSInit(ScrnInfoPtr pScrn)
 	const uint8_t bit_signature[] = { 0xff, 0xb8, 'B', 'I', 'T' };
 	int offset, ret = 0;
 
-	crtc_access(pNv, ACCESS_UNLOCK);
+	NVLockVgaCrtcs(pNv, false);
+	if (pNv->twoHeads)
+		NVSetOwner(pNv, crtchead);
 
 	if ((offset = findstr(pNv->VBIOS.data, pNv->VBIOS.length, bit_signature, sizeof(bit_signature)))) {
 		xf86DrvMsg(pScrn->scrnIndex, X_INFO, "BIT BIOS found\n");
@@ -4492,7 +4480,9 @@ bool NVRunVBIOSInit(ScrnInfoPtr pScrn)
 		ret = 1;
 	}
 
-	crtc_access(pNv, ACCESS_LOCK);
+	NVLockVgaCrtcs(pNv, true);
+	if (pNv->twoHeads)
+		NVSetOwner(pNv, crtchead);
 
 	if (ret)
 		return false;
diff --git a/src/nv_hw.c b/src/nv_hw.c
index 5934c31..698b82e 100644
--- a/src/nv_hw.c
+++ b/src/nv_hw.c
@@ -285,6 +285,14 @@ void NVLockVgaCrtc(NVPtr pNv, int head, bool lock)
 	NVWriteVgaCrtc(pNv, head, NV_CIO_CR_VRE_INDEX, cr11);
 }
 
+void NVLockVgaCrtcs(NVPtr pNv, bool lock)
+{
+	NVLockVgaCrtc(pNv, 0, lock);
+	/* NV11 has independently lockable crtcs */
+	if (pNv->NVArch == 0x11)
+		NVLockVgaCrtc(pNv, 1, lock);
+}
+
 void NVBlankScreen(NVPtr pNv, int head, bool blank)
 {
 	unsigned char seq1;
diff --git a/src/nv_proto.h b/src/nv_proto.h
index 3c5c9e1..aefc0ce 100644
--- a/src/nv_proto.h
+++ b/src/nv_proto.h
@@ -115,6 +115,7 @@ void NVVgaSeqReset(NVPtr pNv, int head, bool start);
 void NVVgaProtect(NVPtr pNv, int head, bool protect);
 void NVSetOwner(NVPtr pNv, int owner);
 void NVLockVgaCrtc(NVPtr pNv, int head, bool lock);
+void NVLockVgaCrtcs(NVPtr pNv, bool lock);
 void NVBlankScreen(NVPtr pNv, int head, bool blank);
 void nv_fix_nv40_hw_cursor(NVPtr pNv, int head);
 void nv_show_cursor(NVPtr pNv, int head, bool show);
diff --git a/src/nv_setup.c b/src/nv_setup.c
index c2a8935..0237662 100644
--- a/src/nv_setup.c
+++ b/src/nv_setup.c
@@ -236,6 +236,55 @@ NVProbeDDC (ScrnInfoPtr pScrn, int bus)
     return MonInfo;
 }
 
+static void store_initial_head_owner(ScrnInfoPtr pScrn)
+{
+	NVPtr pNv = NVPTR(pScrn);
+
+	if (pNv->NVArch != 0x11) {
+		pNv->vtOWNER = NVReadVgaCrtc(pNv, 0, NV_CIO_CRE_44);
+		goto ownerknown;
+	}
+
+	/* reading CR44 is broken on nv11, so we attempt to infer it */
+	if (nvReadMC(pNv, NV_PBUS_DEBUG_1) & (1 << 28))	/* heads tied, restore both */
+		pNv->vtOWNER = 0x04;
+	else {
+		uint8_t slaved_on_A, slaved_on_B;
+		bool tvA, tvB = false;
+
+		NVLockVgaCrtcs(pNv, false);
+
+		slaved_on_B = NVReadVgaCrtc(pNv, 1, NV_CIO_CRE_PIXEL_INDEX) & 0x80;
+		if (slaved_on_B)
+			tvB = !(NVReadVgaCrtc(pNv, 1, NV_CIO_CRE_LCD__INDEX) & NV_CIO_CRE_LCD_LCD_SELECT);
+
+		slaved_on_A = NVReadVgaCrtc(pNv, 0, NV_CIO_CRE_PIXEL_INDEX) & 0x80;
+		if (slaved_on_A)
+			tvA = !(NVReadVgaCrtc(pNv, 0, NV_CIO_CRE_LCD__INDEX) & NV_CIO_CRE_LCD_LCD_SELECT);
+
+		NVLockVgaCrtcs(pNv, true);
+
+		if (slaved_on_A && !tvA)
+			pNv->vtOWNER = 0x0;
+		else if (slaved_on_B && !tvB)
+			pNv->vtOWNER = 0x3;
+		else if (slaved_on_A)
+			pNv->vtOWNER = 0x0;
+		else if (slaved_on_B)
+			pNv->vtOWNER = 0x3;
+		else
+			pNv->vtOWNER = 0x0;
+	}
+
+ownerknown:
+	xf86DrvMsg(pScrn->scrnIndex, X_INFO, "Initial CRTC_OWNER is %d\n", pNv->vtOWNER);
+
+	/* we need to ensure the heads are not tied henceforth, or reading any
+	 * 8 bit reg on head B will fail
+	 * setting a single arbitrary head solves that */
+	NVSetOwner(pNv, 0);
+}
+
 static void nv4GetConfig (NVPtr pNv)
 {
 	uint32_t reg_FB0 = nvReadFB(pNv, NV_PFB_BOOT_0);
@@ -471,46 +520,8 @@ NVCommonSetup(ScrnInfoPtr pScrn)
 
 	pNv->Television = FALSE;
 
-	if (pNv->twoHeads) {
-		pNv->vtOWNER = NVReadVgaCrtc(pNv, 0, NV_CIO_CRE_44);
-		if (pNv->NVArch == 0x11) {	/* reading OWNER is broken on nv11 */
-			if (nvReadMC(pNv, NV_PBUS_DEBUG_1) & (1 << 28))	/* heads tied, restore both */
-				pNv->vtOWNER = 0x04;
-			else {
-				uint8_t slaved_on_A, slaved_on_B;
-
-				NVLockVgaCrtc(pNv, 1, false);
-
-				slaved_on_B = NVReadVgaCrtc(pNv, 1, NV_CIO_CRE_PIXEL_INDEX) & 0x80;
-				if (slaved_on_B)
-					tvB = !(NVReadVgaCrtc(pNv, 1, NV_CIO_CRE_LCD__INDEX) & NV_CIO_CRE_LCD_LCD_SELECT);
-
-				NVLockVgaCrtc(pNv, 0, false);
-
-				slaved_on_A = NVReadVgaCrtc(pNv, 0, NV_CIO_CRE_PIXEL_INDEX) & 0x80;
-				if (slaved_on_A)
-					tvA = !(NVReadVgaCrtc(pNv, 0, NV_CIO_CRE_LCD__INDEX) & NV_CIO_CRE_LCD_LCD_SELECT);
-
-				if (slaved_on_A && !tvA)
-					pNv->vtOWNER = 0x0;
-				else if (slaved_on_B && !tvB)
-					pNv->vtOWNER = 0x3;
-				else if (slaved_on_A)
-					pNv->vtOWNER = 0x0;
-				else if (slaved_on_B)
-					pNv->vtOWNER = 0x3;
-				else
-					pNv->vtOWNER = 0x0;
-			}
-		}
-
-		xf86DrvMsg(pScrn->scrnIndex, X_INFO, "Initial CRTC_OWNER is %d\n", pNv->vtOWNER);
-
-		/* we need to ensure the heads are not tied henceforth, or
-		 * reading any 8 bit reg on head B will fail
-		 * setting a single arbitrary head solves that */
-		NVSetOwner(pNv, 0);
-	}
+	if (pNv->twoHeads)
+		store_initial_head_owner(pScrn);
 
 	/* Parse the bios to initialize the card */
 	NVParseBios(pScrn);