diff --git a/c328.c b/c328.c
index ca73afb..c278b77 100644
--- a/c328.c
+++ b/c328.c
@@ -19,7 +19,7 @@ uint8_t rxbuf[RXBUF_LEN];
 uint16_t rxbuf_len = 0;
 
 /* Expected package size */
-static uint8_t pkg_len = 64; /* Default is 64 according to datasheet */
+static uint16_t pkg_len = 64; /* Default is 64 according to datasheet */
 
 /* Timeout counter */
 volatile static uint8_t timeout_clk = 0;
@@ -156,7 +156,9 @@ char c3_get_picture(uint8_t pt, uint16_t *length)
 char c3_get_package(uint16_t id, uint8_t **dst, uint16_t *length)
 {
 	uint8_t checksum;
-	uint16_t s;
+	volatile uint16_t s;
+	/* s is volatile to work around an apparent bug in avr-gcc --
+	 * discovered by ms7821 in #highaltitude */
 	
 	rxbuf_len = 0;
 	checksum = 0;
@@ -172,7 +174,8 @@ char c3_get_package(uint16_t id, uint8_t **dst, uint16_t *length)
 		if(!RXREADY) continue;
 		
 		/* Read the byte and update checksum */
-		checksum += rxbuf[rxbuf_len++] = UDR0;
+		rxbuf[rxbuf_len] = UDR0;
+		checksum += rxbuf[rxbuf_len++];
 		
 		if(rxbuf_len == 4)
 		{
diff --git a/hadie.c b/hadie.c
index fc04f51..82fc053 100644
--- a/hadie.c
+++ b/hadie.c
@@ -91,6 +91,7 @@ char tx_image(void)
 	static uint16_t pkg_len;
 	
 	static uint8_t img_id = 0;
+	static uint8_t img_tx;
 	static uint8_t pkt_id;
 	
 	if(!setup)
@@ -101,6 +102,8 @@ char tx_image(void)
 		pkt_id = 0;
 		pkg_len = 0;
 		pkg_id = 0;
+		img_tx = 0;
+		img_id++;
 	}
 	
 	/* Initialise the packet -- make sure previous packet has finished TX'ing! */
@@ -114,7 +117,7 @@ char tx_image(void)
 			char msg[100];
 			if(c3_get_package(pkg_id++, &pkg, &pkg_len) != 0)
 			{
-				snprintf(msg, 100, "$$" CALLSIGN ",Get package %i failed, had %i bytes: %02X %02X %02X %02X\n", pkg_id - 1, pkg_len, pkg[0], pkg[1], pkg[2], pkg[3]);
+				snprintf(msg, 100, "$$" CALLSIGN ",Get package %i failed\n", pkg_id - 1);
 				rtx_string(msg);
 				rtx_wait();
 				
@@ -122,10 +125,6 @@ char tx_image(void)
 				return(-1);
 			}
 			
-			snprintf(msg, 100, "$$" CALLSIGN ",Got package %i, %i bytes, %i in pkt\n", pkg_id - 1, pkg_len, pkt_len);
-			rtx_string(msg);
-			rtx_wait();
-			
 			/* Skip the package header */
 			pkg += 4;
 			pkg_len -= 6;
@@ -141,10 +140,18 @@ char tx_image(void)
 			pkg += l;
 			pkg_len -= l;
 			pkt_len += l;
+			img_tx += l;
+		}
+		
+		/* Have we reached the end of the image? */
+		if(img_tx == image_len)
+		{
+			c3_finish_picture();
+			setup = 0;
+			break;
 		}
 	}
 	
-	rtx_string_P(PSTR("TX'ing packet\n"));
 	encode_rs_8(&pkt[1], &pkt[PKT_SIZE_HEADER + PKT_SIZE_PAYLOAD], 0);
 	rtx_data(pkt, PKT_SIZE);