1 files changed, 148 insertions, 41 deletions

diff --git a/sys/dev/vt/hw/ofwfb/ofwfb.c b/sys/dev/vt/hw/ofwfb/ofwfb.c
index b5f8e59..d91feff 100644
--- a/sys/dev/vt/hw/ofwfb/ofwfb.c
+++ b/sys/dev/vt/hw/ofwfb/ofwfb.c
@@ -49,6 +49,10 @@ __FBSDID("$FreeBSD$");
 struct ofwfb_softc {
 	phandle_t	sc_node;
 
+	struct ofw_pci_register sc_pciaddrs[8];
+	int		sc_num_pciaddrs;
+
+
 	intptr_t	sc_addr;
 	int		sc_depth;
 	int		sc_stride;
@@ -58,21 +62,50 @@ struct ofwfb_softc {
 	uint32_t	sc_colormap[16];
 };
 
+static vd_probe_t	ofwfb_probe;
 static vd_init_t	ofwfb_init;
 static vd_blank_t	ofwfb_blank;
 static vd_bitbltchr_t	ofwfb_bitbltchr;
+static vd_fb_mmap_t	ofwfb_mmap;
 
 static const struct vt_driver vt_ofwfb_driver = {
+	.vd_name	= "ofwfb",
+	.vd_probe	= ofwfb_probe,
 	.vd_init	= ofwfb_init,
 	.vd_blank	= ofwfb_blank,
 	.vd_bitbltchr	= ofwfb_bitbltchr,
+	.vd_maskbitbltchr = ofwfb_bitbltchr,
+	.vd_fb_mmap	= ofwfb_mmap,
 	.vd_priority	= VD_PRIORITY_GENERIC+1,
 };
 
 static struct ofwfb_softc ofwfb_conssoftc;
-VT_CONSDEV_DECLARE(vt_ofwfb_driver, PIXEL_WIDTH(1920), PIXEL_HEIGHT(1200),
-    &ofwfb_conssoftc);
-/* XXX: hardcoded max size */
+VT_DRIVER_DECLARE(vt_ofwfb, vt_ofwfb_driver);
+
+static int
+ofwfb_probe(struct vt_device *vd)
+{
+	phandle_t chosen, node;
+	ihandle_t stdout;
+	char type[64];
+
+	chosen = OF_finddevice("/chosen");
+	OF_getprop(chosen, "stdout", &stdout, sizeof(stdout));
+	node = OF_instance_to_package(stdout);
+	if (node == -1) {
+		/*
+		 * The "/chosen/stdout" does not exist try
+		 * using "screen" directly.
+		 */
+		node = OF_finddevice("screen");
+	}
+	OF_getprop(node, "device_type", type, sizeof(type));
+	if (strcmp(type, "display") != 0)
+		return (CN_DEAD);
+
+	/* Looks OK... */
+	return (CN_INTERNAL);
+}
 
 static void
 ofwfb_blank(struct vt_device *vd, term_color_t color)
@@ -109,6 +142,10 @@ ofwfb_bitbltchr(struct vt_device *vd, const uint8_t *src, const uint8_t *mask,
 	uint32_t fgc, bgc;
 	int c;
 	uint8_t b, m;
+	union {
+		uint32_t l;
+		uint8_t	 c[4];
+	} ch1, ch2;
 
 	fgc = sc->sc_colormap[fg];
 	bgc = sc->sc_colormap[bg];
@@ -120,36 +157,70 @@ ofwfb_bitbltchr(struct vt_device *vd, const uint8_t *src, const uint8_t *mask,
 		return;
 
 	line = (sc->sc_stride * top) + left * sc->sc_depth/8;
-	for (; height > 0; height--) {
-		for (c = 0; c < width; c++) {
-			if (c % 8 == 0)
+	if (mask == NULL && sc->sc_depth == 8 && (width % 8 == 0)) {
+		for (; height > 0; height--) {
+			for (c = 0; c < width; c += 8) {
 				b = *src++;
-			else
-				b <<= 1;
-			if (mask != NULL) {
+
+				/*
+				 * Assume that there is more background than
+				 * foreground in characters and init accordingly
+				 */
+				ch1.l = ch2.l = (bg << 24) | (bg << 16) |
+				    (bg << 8) | bg;
+
+				/*
+				 * Calculate 2 x 4-chars at a time, and then
+				 * write these out.
+				 */
+				if (b & 0x80) ch1.c[0] = fg;
+				if (b & 0x40) ch1.c[1] = fg;
+				if (b & 0x20) ch1.c[2] = fg;
+				if (b & 0x10) ch1.c[3] = fg;
+
+				if (b & 0x08) ch2.c[0] = fg;
+				if (b & 0x04) ch2.c[1] = fg;
+				if (b & 0x02) ch2.c[2] = fg;
+				if (b & 0x01) ch2.c[3] = fg;
+
+				*(uint32_t *)(sc->sc_addr + line + c) = ch1.l;
+				*(uint32_t *)(sc->sc_addr + line + c + 4) =
+				    ch2.l;
+			}
+			line += sc->sc_stride;
+		}
+	} else {
+		for (; height > 0; height--) {
+			for (c = 0; c < width; c++) {
 				if (c % 8 == 0)
-					m = *mask++;
+					b = *src++;
 				else
-					m <<= 1;
-				/* Skip pixel write, if mask has no bit set. */
-				if ((m & 0x80) == 0)
-					continue;
-			}
-			switch(sc->sc_depth) {
-			case 8:
-				*(uint8_t *)(sc->sc_addr + line + c) =
-				    b & 0x80 ? fg : bg;
-				break;
-			case 32:
-				*(uint32_t *)(sc->sc_addr + line + 4*c) = 
-				    (b & 0x80) ? fgc : bgc;
-				break;
-			default:
-				/* panic? */
-				break;
+					b <<= 1;
+				if (mask != NULL) {
+					if (c % 8 == 0)
+						m = *mask++;
+					else
+						m <<= 1;
+					/* Skip pixel write, if mask not set. */
+					if ((m & 0x80) == 0)
+						continue;
+				}
+				switch(sc->sc_depth) {
+				case 8:
+					*(uint8_t *)(sc->sc_addr + line + c) =
+					    b & 0x80 ? fg : bg;
+					break;
+				case 32:
+					*(uint32_t *)(sc->sc_addr + line + 4*c)
+					    = (b & 0x80) ? fgc : bgc;
+					break;
+				default:
+					/* panic? */
+					break;
+				}
 			}
+			line += sc->sc_stride;
 		}
-		line += sc->sc_stride;
 	}
 }
 
@@ -217,14 +288,12 @@ ofwfb_initialize(struct vt_device *vd)
 static int
 ofwfb_init(struct vt_device *vd)
 {
-	struct ofwfb_softc *sc = vd->vd_softc;
+	struct ofwfb_softc *sc;
 	char type[64];
 	phandle_t chosen;
 	ihandle_t stdout;
 	phandle_t node;
 	uint32_t depth, height, width;
-	struct ofw_pci_register pciaddrs[8];
-	int n_pciaddrs;
 	uint32_t fb_phys;
 	int i, len;
 #ifdef __sparc64__
@@ -233,6 +302,9 @@ ofwfb_init(struct vt_device *vd)
 	int space;
 #endif
 
+	/* Initialize softc */
+	vd->vd_softc = sc = &ofwfb_conssoftc;
+
 	chosen = OF_finddevice("/chosen");
 	OF_getprop(chosen, "stdout", &stdout, sizeof(stdout));
 	node = OF_instance_to_package(stdout);
@@ -275,15 +347,15 @@ ofwfb_init(struct vt_device *vd)
 	 * child of the PCI device: in that case, try the parent for
 	 * the assigned-addresses property.
 	 */
-	len = OF_getprop(node, "assigned-addresses", pciaddrs,
-	    sizeof(pciaddrs));
+	len = OF_getprop(node, "assigned-addresses", sc->sc_pciaddrs,
+	    sizeof(sc->sc_pciaddrs));
 	if (len == -1) {
 		len = OF_getprop(OF_parent(node), "assigned-addresses",
-		    pciaddrs, sizeof(pciaddrs));
+		    sc->sc_pciaddrs, sizeof(sc->sc_pciaddrs));
         }
         if (len == -1)
                 len = 0;
-	n_pciaddrs = len / sizeof(struct ofw_pci_register);
+	sc->sc_num_pciaddrs = len / sizeof(struct ofw_pci_register);
 
 	/*
 	 * Grab the physical address of the framebuffer, and then map it
@@ -313,13 +385,13 @@ ofwfb_init(struct vt_device *vd)
 		 * Linux does the same thing.
 		 */
 
-		fb_phys = n_pciaddrs;
-		for (i = 0; i < n_pciaddrs; i++) {
+		fb_phys = sc->sc_num_pciaddrs;
+		for (i = 0; i < sc->sc_num_pciaddrs; i++) {
 			/* If it is too small, not the framebuffer */
-			if (pciaddrs[i].size_lo < sc->sc_stride*height)
+			if (sc->sc_pciaddrs[i].size_lo < sc->sc_stride*height)
 				continue;
 			/* If it is not memory, it isn't either */
-			if (!(pciaddrs[i].phys_hi &
+			if (!(sc->sc_pciaddrs[i].phys_hi &
 			    OFW_PCI_PHYS_HI_SPACE_MEM32))
 				continue;
 
@@ -327,11 +399,12 @@ ofwfb_init(struct vt_device *vd)
 			fb_phys = i;
 
 			/* If it is prefetchable, it certainly is */
-			if (pciaddrs[i].phys_hi & OFW_PCI_PHYS_HI_PREFETCHABLE)
+			if (sc->sc_pciaddrs[i].phys_hi &
+			    OFW_PCI_PHYS_HI_PREFETCHABLE)
 				break;
 		}
 
-		if (fb_phys == n_pciaddrs) /* No candidates found */
+		if (fb_phys == sc->sc_num_pciaddrs) /* No candidates found */
 			return (CN_DEAD);
 
 	#if defined(__powerpc__)
@@ -348,3 +421,37 @@ ofwfb_init(struct vt_device *vd)
 	return (CN_INTERNAL);
 }
 
+static int
+ofwfb_mmap(struct vt_device *vd, vm_ooffset_t offset, vm_paddr_t *paddr,
+    int prot, vm_memattr_t *memattr)
+{
+	struct ofwfb_softc *sc = vd->vd_softc;
+        int i;
+
+	/*
+	 * Make sure the requested address lies within the PCI device's
+	 * assigned addrs
+	 */
+	for (i = 0; i < sc->sc_num_pciaddrs; i++)
+	  if (offset >= sc->sc_pciaddrs[i].phys_lo &&
+	    offset < (sc->sc_pciaddrs[i].phys_lo + sc->sc_pciaddrs[i].size_lo))
+		{
+			/*
+			 * If this is a prefetchable BAR, we can (and should)
+			 * enable write-combining.
+			 */
+			if (sc->sc_pciaddrs[i].phys_hi &
+			    OFW_PCI_PHYS_HI_PREFETCHABLE)
+				*memattr = VM_MEMATTR_WRITE_COMBINING;
+
+			*paddr = offset;
+			return (0);
+		}
+
+        /*
+         * Hack for Radeon...
+         */
+	*paddr = offset;
+	return (0);
+}
+