diff --git a/Python/cclib/chip/cc254x.py b/Python/cclib/chip/cc254x.py
index 5e06ac1..3c15a55 100644
--- a/Python/cclib/chip/cc254x.py
+++ b/Python/cclib/chip/cc254x.py
@@ -512,6 +512,11 @@ def writeCODE(self, offset, data, erase=False, verify=False, showProgress=False)
 		WARNING: This requires DMA operations to be unpaused ( use: self.pauseDMA(False) )
 		"""
 
+		# Pad data so that the start and end address are on 4-byte boundaries.
+		data = b"\xff" * (offset % 4) + data
+		data = data + b"\xff" * (-len(data) % 4)
+		offset -= offset % 4
+
 		# Prepare DMA-0 for DEBUG -> RAM (using DBG_BW trigger)
 		self.configDMAChannel( 0, 0x6260, 0x0000, 0x1F, tlen=self.bulkBlockSize, srcInc=0, dstInc=1, priority=1, interrupt=True )
 		# Prepare DMA-1 for RAM -> FLASH (using the FLASH trigger)
@@ -556,17 +561,6 @@ def writeCODE(self, offset, data, erase=False, verify=False, showProgress=False)
 			fAddr = offset + iOfs
 			fPage = int( fAddr / self.flashPageSize )
 
-			# Calculate FLASH address High/Low bytes
-			# for writing (addressable as 32-bit words)
-			fWordOffset = int(fAddr / 4)
-			cHigh = (fWordOffset >> 8) & 0xFF
-			cLow = fWordOffset & 0xFF
-			self.writeXDATA( 0x6271, [cLow, cHigh] )
-
-			# Debug
-			#print "[@%04x: p=%i, ofs=%04x, %02x:%02x]" % (fAddr, fPage, fWordOffset, cHigh, cLow),
-			#sys.stdout.flush()
-
 			# Check if we should erase page first
 			if erase:
 				# Select the page to erase using FADDRH[7:1]
@@ -583,6 +577,13 @@ def writeCODE(self, offset, data, erase=False, verify=False, showProgress=False)
 				while self.isFlashBusy():
 					time.sleep(0.010)
 
+			# Calculate FLASH address High/Low bytes
+			# for writing (addressable as 32-bit words)
+			fWordOffset = int(fAddr / 4)
+			cHigh = (fWordOffset >> 8) & 0xFF
+			cLow = fWordOffset & 0xFF
+			self.writeXDATA( 0x6271, [cLow, cHigh] )
+
 			# Upload to FLASH through DMA-1
 			self.armDMAChannel(1)
 			self.setFlashWrite()