4 files changed, 136 insertions, 42 deletions

diff --git a/drivers/video/nvidia/nv_of.c b/drivers/video/nvidia/nv_of.c
index 4fa2cf9a8af2..7a03d040b1a3 100644
--- a/drivers/video/nvidia/nv_of.c
+++ b/drivers/video/nvidia/nv_of.c
@@ -27,34 +27,60 @@
 #include "nv_local.h"
 #include "nv_proto.h"
 
-void nvidia_create_i2c_busses(struct nvidia_par *par) {}
-void nvidia_delete_i2c_busses(struct nvidia_par *par) {}
+#include "../edid.h"
 
-int nvidia_probe_i2c_connector(struct fb_info *info, int conn, u8 **out_edid)
+int nvidia_probe_of_connector(struct fb_info *info, int conn, u8 **out_edid)
 {
         struct nvidia_par *par = info->par;
-        struct device_node *dp;
+        struct device_node *parent, *dp;
         unsigned char *pedid = NULL;
-        unsigned char *disptype = NULL;
         static char *propnames[] = {
-                "DFP,EDID", "LCD,EDID", "EDID", "EDID1", "EDID,B", "EDID,A", NULL };
+                "DFP,EDID", "LCD,EDID", "EDID", "EDID1",
+                "EDID,B", "EDID,A", NULL };
         int i;
 
-        dp = pci_device_to_OF_node(par->pci_dev);
-        for (; dp != NULL; dp = dp->child) {
-                disptype = (unsigned char *)get_property(dp, "display-type", NULL);
-                if (disptype == NULL)
-                        continue;
-                if (strncmp(disptype, "LCD", 3) != 0)
-                        continue;
+        parent = pci_device_to_OF_node(par->pci_dev);
+        if (parent == NULL)
+                return -1;
+        if (par->twoHeads) {
+                char *pname;
+                int len;
+
+                for (dp = NULL;
+                     (dp = of_get_next_child(parent, dp)) != NULL;) {
+                        pname = (char *)get_property(dp, "name", NULL);
+                        if (!pname)
+                                continue;
+                        len = strlen(pname);
+                        if ((pname[len-1] == 'A' && conn == 1) ||
+                            (pname[len-1] == 'B' && conn == 2)) {
+                                for (i = 0; propnames[i] != NULL; ++i) {
+                                        pedid = (unsigned char *)
+                                                get_property(dp, propnames[i],
+                                                             NULL);
+                                        if (pedid != NULL)
+                                                break;
+                                }
+                                of_node_put(dp);
+                                break;
+                        }
+                }
+        }
+        if (pedid == NULL) {
                 for (i = 0; propnames[i] != NULL; ++i) {
                         pedid = (unsigned char *)
-                                get_property(dp, propnames[i], NULL);
-                        if (pedid != NULL) {
-                                *out_edid = pedid;
-                                return 0;
-                        }
+                                get_property(parent, propnames[i], NULL);
+                        if (pedid != NULL)
+                                break;
                 }
         }
-        return 1;
+        if (pedid) {
+                *out_edid = kmalloc(EDID_LENGTH, GFP_KERNEL);
+                if (*out_edid == NULL)
+                        return -1;
+                memcpy(*out_edid, pedid, EDID_LENGTH);
+                printk(KERN_DEBUG "nvidiafb: Found OF EDID for head %d\n", conn);
+                return 0;
+        }
+        return -1;
 }
diff --git a/drivers/video/nvidia/nv_proto.h b/drivers/video/nvidia/nv_proto.h
index cac44fc7f587..3d35b77a4af5 100644
--- a/drivers/video/nvidia/nv_proto.h
+++ b/drivers/video/nvidia/nv_proto.h
@@ -31,7 +31,7 @@ int NVShowHideCursor(struct nvidia_par *par, int);
 void NVLockUnlock(struct nvidia_par *par, int);
 
 /* in nvidia-i2c.c */
-#if defined(CONFIG_FB_NVIDIA_I2C) || defined (CONFIG_PPC_OF)
+#ifdef CONFIG_FB_NVIDIA_I2C
 void nvidia_create_i2c_busses(struct nvidia_par *par);
 void nvidia_delete_i2c_busses(struct nvidia_par *par);
 int nvidia_probe_i2c_connector(struct fb_info *info, int conn,
@@ -39,10 +39,14 @@ int nvidia_probe_i2c_connector(struct fb_info *info, int conn,
 #else
 #define nvidia_create_i2c_busses(...)
 #define nvidia_delete_i2c_busses(...)
-#define nvidia_probe_i2c_connector(p, c, edid) \
-do {                                           \
-        *(edid) = NULL;                        \
-} while(0)
+#define nvidia_probe_i2c_connector(p, c, edid) (-1)
+#endif
+
+#ifdef CONFIG_FB_OF
+int nvidia_probe_of_connector(struct fb_info *info, int conn,
+                              u8 ** out_edid);
+#else
+#define nvidia_probe_of_connector(p, c, edid)  (-1)
 #endif
 
 /* in nv_accel.c */
diff --git a/drivers/video/nvidia/nv_setup.c b/drivers/video/nvidia/nv_setup.c
index 11c84178f420..1f06a9f1bd0f 100644
--- a/drivers/video/nvidia/nv_setup.c
+++ b/drivers/video/nvidia/nv_setup.c
@@ -190,9 +190,9 @@ static int NVIsConnected(struct nvidia_par *par, int output)
         present = (NV_RD32(PRAMDAC, 0x0608) & (1 << 28)) ? 1 : 0;
 
         if (present)
-                printk("nvidiafb: CRTC%i found\n", output);
+                printk("nvidiafb: CRTC%i analog found\n", output);
         else
-                printk("nvidiafb: CRTC%i not found\n", output);
+                printk("nvidiafb: CRTC%i analog not found\n", output);
 
         NV_WR32(par->PRAMDAC0, 0x0608, NV_RD32(par->PRAMDAC0, 0x0608) &
                 0x0000EFFF);
@@ -305,6 +305,9 @@ void NVCommonSetup(struct fb_info *info)
         int FlatPanel = -1;     /* really means the CRTC is slaved */
         int Television = 0;
 
+        memset(&monitorA, 0, sizeof(struct fb_monspecs));
+        memset(&monitorB, 0, sizeof(struct fb_monspecs));
+
         par->PRAMIN = par->REGS + (0x00710000 / 4);
         par->PCRTC0 = par->REGS + (0x00600000 / 4);
         par->PRAMDAC0 = par->REGS + (0x00680000 / 4);
@@ -401,7 +404,8 @@ void NVCommonSetup(struct fb_info *info)
         nvidia_create_i2c_busses(par);
         if (!par->twoHeads) {
                 par->CRTCnumber = 0;
-                nvidia_probe_i2c_connector(info, 1, &edidA);
+                if (nvidia_probe_i2c_connector(info, 1, &edidA))
+                        nvidia_probe_of_connector(info, 1, &edidA);
                 if (edidA && !fb_parse_edid(edidA, &var)) {
                         printk("nvidiafb: EDID found from BUS1\n");
                         monA = &monitorA;
@@ -488,14 +492,16 @@ void NVCommonSetup(struct fb_info *info)
                 oldhead = NV_RD32(par->PCRTC0, 0x00000860);
                 NV_WR32(par->PCRTC0, 0x00000860, oldhead | 0x00000010);
 
-                nvidia_probe_i2c_connector(info, 1, &edidA);
+                if (nvidia_probe_i2c_connector(info, 1, &edidA))
+                        nvidia_probe_of_connector(info, 1, &edidA);
                 if (edidA && !fb_parse_edid(edidA, &var)) {
                         printk("nvidiafb: EDID found from BUS1\n");
                         monA = &monitorA;
                         fb_edid_to_monspecs(edidA, monA);
                 }
 
-                nvidia_probe_i2c_connector(info, 2, &edidB);
+                if (nvidia_probe_i2c_connector(info, 2, &edidB))
+                        nvidia_probe_of_connector(info, 2, &edidB);
                 if (edidB && !fb_parse_edid(edidB, &var)) {
                         printk("nvidiafb: EDID found from BUS2\n");
                         monB = &monitorB;
diff --git a/drivers/video/nvidia/nvidia.c b/drivers/video/nvidia/nvidia.c
index 308defc389a2..96d51144094a 100644
--- a/drivers/video/nvidia/nvidia.c
+++ b/drivers/video/nvidia/nvidia.c
@@ -627,41 +627,85 @@ static void nvidia_save_vga(struct nvidia_par *par,
         NVTRACE_LEAVE();
 }
 
+#undef DUMP_REG
+
 static void nvidia_write_regs(struct nvidia_par *par)
 {
         struct _riva_hw_state *state = &par->ModeReg;
         int i;
 
         NVTRACE_ENTER();
-        NVWriteCrtc(par, 0x11, 0x00);
-
-        NVLockUnlock(par, 0);
 
         NVLoadStateExt(par, state);
 
         NVWriteMiscOut(par, state->misc_output);
 
+        for (i = 1; i < NUM_SEQ_REGS; i++) {
+#ifdef DUMP_REG
+                printk(" SEQ[%02x] = %08x\n", i, state->seq[i]);
+#endif
+                NVWriteSeq(par, i, state->seq[i]);
+        }
+
+        /* Ensure CRTC registers 0-7 are unlocked by clearing bit 7 of CRTC[17] */
+        NVWriteCrtc(par, 0x11, state->crtc[0x11] & ~0x80);
+
         for (i = 0; i < NUM_CRT_REGS; i++) {
                 switch (i) {
                 case 0x19:
                 case 0x20 ... 0x40:
                         break;
                 default:
+#ifdef DUMP_REG
+                        printk("CRTC[%02x] = %08x\n", i, state->crtc[i]);
+#endif
                         NVWriteCrtc(par, i, state->crtc[i]);
                 }
         }
 
-        for (i = 0; i < NUM_ATC_REGS; i++)
-                NVWriteAttr(par, i, state->attr[i]);
-
-        for (i = 0; i < NUM_GRC_REGS; i++)
+        for (i = 0; i < NUM_GRC_REGS; i++) {
+#ifdef DUMP_REG
+                printk(" GRA[%02x] = %08x\n", i, state->gra[i]);
+#endif
                 NVWriteGr(par, i, state->gra[i]);
+        }
+
+        for (i = 0; i < NUM_ATC_REGS; i++) {
+#ifdef DUMP_REG
+                printk("ATTR[%02x] = %08x\n", i, state->attr[i]);
+#endif
+                NVWriteAttr(par, i, state->attr[i]);
+        }
 
-        for (i = 0; i < NUM_SEQ_REGS; i++)
-                NVWriteSeq(par, i, state->seq[i]);
         NVTRACE_LEAVE();
 }
 
+static void nvidia_vga_protect(struct nvidia_par *par, int on)
+{
+        unsigned char tmp;
+
+        if (on) {
+                /*
+                 * Turn off screen and disable sequencer.
+                 */
+                tmp = NVReadSeq(par, 0x01);
+
+                NVWriteSeq(par, 0x00, 0x01);            /* Synchronous Reset */
+                NVWriteSeq(par, 0x01, tmp | 0x20);      /* disable the display */
+        } else {
+                /*
+                 * Reenable sequencer, then turn on screen.
+                 */
+
+                tmp = NVReadSeq(par, 0x01);
+
+                NVWriteSeq(par, 0x01, tmp & ~0x20);     /* reenable display */
+                NVWriteSeq(par, 0x00, 0x03);            /* End Reset */
+        }
+}
+
+
+
 static int nvidia_calc_regs(struct fb_info *info)
 {
         struct nvidia_par *par = info->par;
@@ -868,7 +912,7 @@ static void nvidia_init_vga(struct fb_info *info)
         for (i = 0; i < 0x10; i++)
                 state->attr[i] = i;
         state->attr[0x10] = 0x41;
-        state->attr[0x11] = 0x01;
+        state->attr[0x11] = 0xff;
         state->attr[0x12] = 0x0f;
         state->attr[0x13] = 0x00;
         state->attr[0x14] = 0x00;
@@ -991,7 +1035,6 @@ static int nvidiafb_set_par(struct fb_info *info)
 
         nvidia_init_vga(info);
         nvidia_calc_regs(info);
-        nvidia_write_regs(par);
 
         NVLockUnlock(par, 0);
         if (par->twoHeads) {
@@ -1000,7 +1043,22 @@ static int nvidiafb_set_par(struct fb_info *info)
                 NVLockUnlock(par, 0);
         }
 
-        NVWriteCrtc(par, 0x11, 0x00);
+        nvidia_vga_protect(par, 1);
+
+        nvidia_write_regs(par);
+
+#if defined (__BIG_ENDIAN)
+        /* turn on LFB swapping */
+        {
+                unsigned char tmp;
+
+                VGA_WR08(par->PCIO, 0x3d4, 0x46);
+                tmp = VGA_RD08(par->PCIO, 0x3d5);
+                tmp |= (1 << 7);
+                VGA_WR08(par->PCIO, 0x3d5, tmp);
+    }
+#endif
+
         info->fix.line_length = (info->var.xres_virtual *
                                  info->var.bits_per_pixel) >> 3;
         if (info->var.accel_flags) {
@@ -1022,7 +1080,7 @@ static int nvidiafb_set_par(struct fb_info *info)
 
         par->cursor_reset = 1;
 
-        NVWriteCrtc(par, 0x11, 0xff);
+        nvidia_vga_protect(par, 0);
 
         NVTRACE_LEAVE();
         return 0;