diff --git a/examples/board_usi_i2cslave/attiny85_i2cslave.c b/examples/board_usi_i2cslave/attiny85_i2cslave.c
index cf5ce43e4..634b397f4 100644
--- a/examples/board_usi_i2cslave/attiny85_i2cslave.c
+++ b/examples/board_usi_i2cslave/attiny85_i2cslave.c
@@ -215,7 +215,7 @@ static inline void SET_USI_TO_RECEIVE_ACK()
 	i2cStatus = USII2CSLV_RECEIVING_ACK_FOR_TX;
 	DDR_USI	&= ~(1 << PORT_USI_SDA);
 	PORT_USI|=	_BV(PORT_USI_SDA);
-	USIDR	=	0x80;
+	USIDR	=	0;
 	USISR	=	(0 << USISIF)   | (1 << USIOIF) | (1 << USIPF) |
 				(1 << USIDC) | (0x0E << USICNT0);
 }
diff --git a/simavr/sim/avr_ioport.c b/simavr/sim/avr_ioport.c
index 317cf7931..a6685653c 100644
--- a/simavr/sim/avr_ioport.c
+++ b/simavr/sim/avr_ioport.c
@@ -181,7 +181,8 @@ avr_ioport_irq_notify(
 			 */
 
 			avr->data[p->r_port] = new_out;
-			return;	   // TODO: stop further processing of IRQ.
+			irq->flags |= IRQ_FLAG_NTF_STOP; // Stop further processing of IRQ.
+			return;
 		}
 	} else {
 		// Set the real PIN bit.
diff --git a/simavr/sim/sim_io.c b/simavr/sim/sim_io.c
index 548f03061..28e92fbdd 100644
--- a/simavr/sim/sim_io.c
+++ b/simavr/sim/sim_io.c
@@ -48,9 +48,16 @@ avr_register_io(
 		avr_t *avr,
 		avr_io_t * io)
 {
-	io->next = avr->io_port;
 	io->avr = avr;
-	avr->io_port = io;
+	io->next = NULL;
+	if (!avr->io_port) {
+		avr->io_port = io;
+	} else {
+		avr_io_t * port = avr->io_port;
+		while (port->next)
+			port = port->next;
+		port->next = io;
+	}
 }
 
 void
diff --git a/simavr/sim/sim_irq.c b/simavr/sim/sim_irq.c
index f0656bc11..276e5b753 100644
--- a/simavr/sim/sim_irq.c
+++ b/simavr/sim/sim_irq.c
@@ -105,12 +105,14 @@ avr_alloc_irq(
 
 static avr_irq_hook_t *
 _avr_alloc_irq_hook(
-		avr_irq_t * irq)
+		avr_irq_hook_t ** parent)
 {
 	avr_irq_hook_t *hook = malloc(sizeof(avr_irq_hook_t));
 	memset(hook, 0, sizeof(avr_irq_hook_t));
-	hook->next = irq->hook;
-	irq->hook = hook;
+	if (*parent)
+		(*parent)->next = hook;
+	else
+		(*parent) = hook;
 	return hook;
 }
 
@@ -152,17 +154,23 @@ avr_irq_register_notify(
 		return;
 
 	avr_irq_hook_t *hook = irq->hook;
-	while (hook) {
-		if (hook->notify == notify && hook->param == param) {
-			if (irq->pool->avr->resetting) {
-				irq->value = 0;
-				irq->flags |= IRQ_FLAG_INIT;
+	if (!hook) {
+		hook = _avr_alloc_irq_hook(&(irq->hook));
+	} else {
+		do {
+			if (hook->notify == notify && hook->param == param) {
+				if (irq->pool->avr->resetting) {
+					irq->value = 0;
+					irq->flags |= IRQ_FLAG_INIT;
+				}
+				return;	// already there
 			}
-			return;	// already there
-		}
-		hook = hook->next;
+			if (!hook->next)
+				break;
+			hook = hook->next;
+		} while (1);
+		hook = _avr_alloc_irq_hook(&hook);
 	}
-	hook = _avr_alloc_irq_hook(irq);
 	hook->notify = notify;
 	hook->param = param;
 }
@@ -217,8 +225,14 @@ avr_raise_irq_float(
 			// prevents reentrance / endless calling loops
 		if (hook->busy == 0) {
 			hook->busy++;
-			if (hook->notify)
+			if (hook->notify) {
 				hook->notify(irq, output,  hook->param);
+				if (irq->flags & IRQ_FLAG_NTF_STOP) {
+					irq->flags &= ~IRQ_FLAG_NTF_STOP;
+					hook->busy--;
+					return; //Stop immediately, leaving irq->value untouched
+				}
+			}
 			if (hook->chain)
 				avr_raise_irq_float(hook->chain, output, floating);
 			hook->busy--;
@@ -249,12 +263,18 @@ avr_connect_irq(
 		return;
 	}
 	avr_irq_hook_t *hook = src->hook;
-	while (hook) {
-		if (hook->chain == dst)
-			return;	// already there
-		hook = hook->next;
+	if (!hook) {
+		hook = _avr_alloc_irq_hook(&(src->hook));
+	} else {
+		do {
+			if (hook->chain == dst)
+				return;	// already there
+			if (!hook->next)
+				break;
+			hook = hook->next;
+		} while (1);
+		hook = _avr_alloc_irq_hook(&hook);
 	}
-	hook = _avr_alloc_irq_hook(src);
 	hook->chain = dst;
 }
 
diff --git a/simavr/sim/sim_irq.h b/simavr/sim/sim_irq.h
index 01cf9e4c2..e0663d8e9 100644
--- a/simavr/sim/sim_irq.h
+++ b/simavr/sim/sim_irq.h
@@ -56,6 +56,7 @@ enum {
 	IRQ_FLAG_FILTERED	= (1 << 1),	//!< do not "notify" if "value" is the same as previous raise
 	IRQ_FLAG_ALLOC		= (1 << 2), //!< this irq structure was malloced via avr_alloc_irq
 	IRQ_FLAG_INIT		= (1 << 3), //!< this irq hasn't been used yet
+	IRQ_FLAG_NTF_STOP	= (1 << 3), //!< only for notify functions - stop processing hooks after current notify
 	IRQ_FLAG_FLOATING	= (1 << 4), //!< this 'pin'/signal is floating
 	IRQ_FLAG_STRONG		= (1 << 5), //!< this 'pin'/signal is held at current state, do not "notify"
 	IRQ_FLAG_USER		= (1 << 6), //!< Can be used by irq users