1 files changed, 37 insertions, 4 deletions

diff --git a/sys/amd64/vmm/io/vatpic.c b/sys/amd64/vmm/io/vatpic.c
index c6d4590..74a7027 100644
--- a/sys/amd64/vmm/io/vatpic.c
+++ b/sys/amd64/vmm/io/vatpic.c
@@ -112,6 +112,16 @@ struct vatpic {
 
 static void vatpic_set_pinstate(struct vatpic *vatpic, int pin, bool newstate);
 
+static __inline bool
+master_atpic(struct vatpic *vatpic, struct atpic *atpic)
+{
+
+	if (atpic == &vatpic->atpic[0])
+		return (true);
+	else
+		return (false);
+}
+
 static __inline int
 vatpic_get_highest_isrpin(struct atpic *atpic)
 {
@@ -250,6 +260,7 @@ vatpic_icw1(struct vatpic *vatpic, struct atpic *atpic, uint8_t val)
 	atpic->mask = 0;
 	atpic->lowprio = 7;
 	atpic->rd_cmd_reg = 0;
+	atpic->poll = 0;
 
 	if ((val & ICW1_SNGL) != 0) {
 		VATPIC_CTR0(vatpic, "vatpic cascade mode required");
@@ -301,6 +312,15 @@ vatpic_icw4(struct vatpic *vatpic, struct atpic *atpic, uint8_t val)
 	if ((val & ICW4_AEOI) != 0)
 		atpic->aeoi = true;
 
+	if ((val & ICW4_SFNM) != 0) {
+		if (master_atpic(vatpic, atpic)) {
+			atpic->sfn = true;
+		} else {
+			VATPIC_CTR1(vatpic, "Ignoring special fully nested "
+			    "mode on slave atpic: %#x", val);
+		}
+	}
+
 	atpic->icw_num = 0;
 	atpic->ready = true;
 
@@ -354,11 +374,17 @@ vatpic_ocw3(struct vatpic *vatpic, struct atpic *atpic, uint8_t val)
 {
 	VATPIC_CTR1(vatpic, "atpic ocw3 0x%x", val);
 
-	atpic->poll = ((val & OCW3_P) != 0);
+	if (val & OCW3_ESMM) {
+		VATPIC_CTR0(vatpic, "atpic special mask mode not implemented");
+		return (-1);
+	}
 
 	if (val & OCW3_RR) {
 		/* read register command */
 		atpic->rd_cmd_reg = val & OCW3_RIS;
+
+		/* Polling mode */
+		atpic->poll = ((val & OCW3_P) != 0);
 	}
 
 	return (0);
@@ -578,12 +604,19 @@ static int
 vatpic_read(struct vatpic *vatpic, struct atpic *atpic, bool in, int port,
 	    int bytes, uint32_t *eax)
 {
+	int pin;
+
 	VATPIC_LOCK(vatpic);
 
 	if (atpic->poll) {
-		VATPIC_CTR0(vatpic, "vatpic polled mode not supported");
-		VATPIC_UNLOCK(vatpic);
-		return (-1);
+		atpic->poll = 0;
+		pin = vatpic_get_highest_irrpin(atpic);
+		if (pin >= 0) {
+			vatpic_pin_accepted(atpic, pin);
+			*eax = 0x80 | pin;
+		} else {
+			*eax = 0;
+		}
 	} else {
 		if (port & ICU_IMR_OFFSET) {
 			/* read interrrupt mask register */