diff --git a/kernel/busses/i2c-keywest.c b/kernel/busses/i2c-keywest.c
index a74456f6..7f218f17 100644
--- a/kernel/busses/i2c-keywest.c
+++ b/kernel/busses/i2c-keywest.c
@@ -65,8 +65,6 @@
 
 #include "i2c-keywest.h"
 
-#undef POLLED_MODE
-
 #define DBG(x...) do {\
 	if (debug > 0) \
 		printk(KERN_DEBUG "KW:" x); \
@@ -85,27 +83,6 @@ int debug = 0;
 
 static struct keywest_iface *ifaces = NULL;
 
-#ifdef POLLED_MODE
-/* This isn't fast, but will go once I implement interrupt with
- * proper timeout
- */
-static u8
-wait_interrupt(struct keywest_iface* iface)
-{
-	int i;
-	u8 isr;
-	
-	for (i = 0; i < POLL_TIMEOUT; i++) {
-		isr = read_reg(reg_isr) & KW_I2C_IRQ_MASK;
-		if (isr != 0)
-			return isr;
-		current->state = TASK_UNINTERRUPTIBLE;
-		schedule_timeout(1);
-	}
-	return isr;
-}
-#endif /* POLLED_MODE */
-
 
 static void
 do_stop(struct keywest_iface* iface, int result)
@@ -116,16 +93,17 @@ do_stop(struct keywest_iface* iface, int result)
 }
 
 /* Main state machine for standard & standard sub mode */
-static void
+static int
 handle_interrupt(struct keywest_iface *iface, u8 isr)
 {
 	int ack;
+	int rearm_timer = 1;
 	
 	DBG("handle_interrupt(), got: %x, status: %x, state: %d\n",
 		isr, read_reg(reg_status), iface->state);
 	if (isr == 0 && iface->state != state_stop) {
 		do_stop(iface, -1);
-		return;
+		return rearm_timer;
 	}
 	if (isr & KW_I2C_IRQ_STOP && iface->state != state_stop) {
 		iface->result = -1;
@@ -196,20 +174,19 @@ handle_interrupt(struct keywest_iface *iface, u8 isr)
 		if (!(isr & KW_I2C_IRQ_STOP) && (++iface->stopretry) < 10)
 			do_stop(iface, -1);
 		else {
+			rearm_timer = 0;
 			iface->state = state_idle;
 			write_reg(reg_control, 0x00);
 			write_reg(reg_ier, 0x00);
-#ifndef POLLED_MODE
 			complete(&iface->complete);
-#endif /* POLLED_MODE */			
 		}
 		break;
 	}
 	
 	write_reg(reg_isr, isr);
-}
 
-#ifndef POLLED_MODE
+	return rearm_timer;
+}
 
 /* Interrupt handler */
 static void
@@ -219,8 +196,7 @@ keywest_irq(int irq, void *dev_id, struct pt_regs *regs)
 
 	spin_lock(&iface->lock);
 	del_timer(&iface->timeout_timer);
-	handle_interrupt(iface, read_reg(reg_isr));
-	if (iface->state != state_idle) {
+	if (handle_interrupt(iface, read_reg(reg_isr))) {
 		iface->timeout_timer.expires = jiffies + POLL_TIMEOUT;
 		add_timer(&iface->timeout_timer);
 	}
@@ -234,7 +210,6 @@ keywest_timeout(unsigned long data)
 
 	DBG("timeout !\n");
 	spin_lock_irq(&iface->lock);
-	handle_interrupt(iface, read_reg(reg_isr));
 	if (iface->state != state_idle) {
 		iface->timeout_timer.expires = jiffies + POLL_TIMEOUT;
 		add_timer(&iface->timeout_timer);
@@ -242,8 +217,6 @@ keywest_timeout(unsigned long data)
 	spin_unlock(&iface->lock);
 }
 
-#endif /* POLLED_MODE */
-
 /*
  * SMBUS-type transfer entrypoint
  */
@@ -340,17 +313,7 @@ keywest_smbus_xfer(	struct i2c_adapter*	adap,
 	write_reg(reg_control, read_reg(reg_control) | KW_I2C_CTL_XADDR);
 	write_reg(reg_ier, KW_I2C_IRQ_MASK);
 
-#ifdef POLLED_MODE
-	DBG("using polled mode...\n");
-	/* State machine, to turn into an interrupt handler */
-	while(iface->state != state_idle) {
-		u8 isr = wait_interrupt(iface);
-		handle_interrupt(iface, isr);
-	}
-#else /* POLLED_MODE */
-	DBG("using interrupt mode...\n");
 	wait_for_completion(&iface->complete);	
-#endif /* POLLED_MODE */	
 
 	rc = iface->result;	
 	DBG("transfer done, result: %d\n", rc);
@@ -430,17 +393,7 @@ keywest_xfer(	struct i2c_adapter *adap,
 		write_reg(reg_control, read_reg(reg_control) | KW_I2C_CTL_XADDR);
 		write_reg(reg_ier, KW_I2C_IRQ_MASK);
 
-#ifdef POLLED_MODE
-		DBG("using polled mode...\n");
-		/* State machine, to turn into an interrupt handler */
-		while(iface->state != state_idle) {
-			u8 isr = wait_interrupt(iface);
-			handle_interrupt(iface, isr);
-		}
-#else /* POLLED_MODE */
-		DBG("using interrupt mode...\n");
 		wait_for_completion(&iface->complete);	
-#endif /* POLLED_MODE */	
 
 		rc = iface->result;
 		if (rc == 0)
@@ -564,7 +517,6 @@ create_iface(struct device_node* np)
 	write_reg(reg_ier, 0x00);
 	write_reg(reg_isr, KW_I2C_IRQ_MASK);
 
-#ifndef POLLED_MODE
 	/* Request chip interrupt */	
 	rc = request_irq(iface->irq, keywest_irq, 0, "keywest i2c", iface);
 	if (rc) {
@@ -573,7 +525,6 @@ create_iface(struct device_node* np)
 		kfree(iface);
 		return -ENODEV;
 	}
-#endif /* POLLED_MODE */
 
 	for (i=0; i<nchan; i++) {
 		struct keywest_chan* chan = &iface->channels[i];
@@ -625,19 +576,16 @@ dispose_iface(struct keywest_iface *iface)
 
 	/* Make sure we stop all activity */
 	down(&iface->sem);
-#ifndef POLLED_MODE
 	spin_lock_irq(&iface->lock);
 	while (iface->state != state_idle) {
 		spin_unlock_irq(&iface->lock);
-		schedule();
+		set_task_state(current,TASK_UNINTERRUPTIBLE);
+		schedule_timeout(HZ/10);
 		spin_lock_irq(&iface->lock);
 	}
-#endif /* POLLED_MODE */
 	iface->state = state_dead;
-#ifndef POLLED_MODE
 	spin_unlock_irq(&iface->lock);
 	free_irq(iface->irq, iface);
-#endif /* POLLED_MODE */
 	up(&iface->sem);
 
 	/* Release all channels */