2 files changed, 65 insertions, 0 deletions

diff --git a/sys/dev/ata/ata-dma.c b/sys/dev/ata/ata-dma.c
index da9770b8d1b2..5ee516de00b1 100644
--- a/sys/dev/ata/ata-dma.c
+++ b/sys/dev/ata/ata-dma.c
@@ -749,6 +749,49 @@ via_82c586:
 	atadev->mode = ATA_PIO0 + apiomode;
 	return;
 
+    case 0x02121166:	/* ServerWorks CSB5 ATA66/100 controller */
+	if (udmamode >= 5 && pci_get_revid(parent) >= 0x92) {
+	    error = ata_command(atadev, ATA_C_SETFEATURES, 0,
+				ATA_UDMA5, ATA_C_F_SETXFER, ATA_WAIT_READY);
+	    if (bootverbose)
+		ata_prtdev(atadev, "%s setting UDMA5 on ServerWorks chip\n",
+			   (error) ? "failed" : "success");
+	    if (!error) {
+		u_int16_t reg56;
+
+		pci_write_config(parent, 0x54, 
+				 pci_read_config(parent, 0x54, 1) |
+				 (0x01 << devno), 1);
+		reg56 = pci_read_config(parent, 0x56, 2);
+		reg56 &= ~(0xf << (devno * 4));
+		reg56 |= (0x5 << (devno * 4));
+		pci_write_config(parent, 0x56, reg56, 2);
+		atadev->mode = ATA_UDMA5;
+		return;
+	    }
+	}
+	if (udmamode >= 4) {
+	    error = ata_command(atadev, ATA_C_SETFEATURES, 0,
+				ATA_UDMA4, ATA_C_F_SETXFER, ATA_WAIT_READY);
+	    if (bootverbose)
+		ata_prtdev(atadev, "%s setting UDMA4 on ServerWorks chip\n",
+			   (error) ? "failed" : "success");
+	    if (!error) {
+		u_int16_t reg56;
+
+		pci_write_config(parent, 0x54, 
+				 pci_read_config(parent, 0x54, 1) |
+				 (0x01 << devno), 1);
+		reg56 = pci_read_config(parent, 0x56, 2);
+		reg56 &= ~(0xf << (devno * 4));
+		reg56 |= (0x4 << (devno * 4));
+		pci_write_config(parent, 0x56, reg56, 2);
+		atadev->mode = ATA_UDMA4;
+		return;
+	    }
+	}
+	/* FALLTHROUGH */
+
     case 0x02111166:	/* ServerWorks ROSB4 ATA33 controller */
 	if (udmamode >= 2) {
 	    error = ata_command(atadev, ATA_C_SETFEATURES, 0,
diff --git a/sys/dev/ata/ata-pci.c b/sys/dev/ata/ata-pci.c
index 1150b2f61058..76b0906304a9 100644
--- a/sys/dev/ata/ata-pci.c
+++ b/sys/dev/ata/ata-pci.c
@@ -231,6 +231,12 @@ ata_pci_match(device_t dev)
     case 0x02111166:
 	return "ServerWorks ROSB4 ATA33 controller";
 
+    case 0x02121166:
+	if (pci_get_revid(dev) >= 0x92)
+	    return "ServerWorks CSB5 ATA100 controller";
+	else
+	    return "ServerWorks CSB5 ATA66 controller";
+
     case 0x4d33105a:
 	return "Promise ATA33 controller";
 
@@ -461,6 +467,22 @@ ata_pci_attach(device_t dev)
 	pci_write_config(dev, 0x68, DEV_BSIZE, 2);
 	break;
 
+    case 0x02111166: /* ServerWorks ROSB4 enable UDMA33 */
+	pci_write_config(dev, 0x64,   
+			 (pci_read_config(dev, 0x64, 4) & ~0x00002000) |
+			 0x00004000, 4);
+	break;
+	
+    case 0x02121166: /* ServerWorks CSB5 enable UDMA66/100 depending on rev */
+	pci_write_config(dev, 0x5a,   
+			 (pci_read_config(dev, 0x5a, 1) & ~0x40) |
+			 (pci_get_revid(dev) >= 0x92) ? 0x03 : 0x02, 1);
+	break;
+
+    case 0x06461095: /* CMD 646 enable interrupts, set DMA read mode */
+	pci_write_config(dev, 0x71, 0x01, 1);
+	break;
+
     case 0x10001042:   /* RZ 100? known bad, no DMA */
     case 0x10011042:
     case 0x06401095:   /* CMD 640 known bad, no DMA */