2 files changed, 8 insertions, 72 deletions

diff --git a/cpu/mpc824x/drivers/i2c/i2c1.c b/cpu/mpc824x/drivers/i2c/i2c1.c
index dc1c97ca0f..94c671e567 100644
--- a/cpu/mpc824x/drivers/i2c/i2c1.c
+++ b/cpu/mpc824x/drivers/i2c/i2c1.c
@@ -1197,13 +1197,16 @@ int i2c_write (uchar chip, uint addr, int alen, uchar * buffer, int len)
 	uchar *p;
 	int i;
 
-	p = dummy_buffer;
 	/* fill in address in big endian order */
-	for (i=0; i<alen; ++i)
-		*p++ = (addr >> (i * 8)) & 0xFF;
+	for (i=alen-1; i>=0; --i) {
+		buffer[i] = addr & 0xFF;
+		addr >>= 8;
+	}
 	/* fill in data */
+	p = dummy_buffer + alen;
+
 	for (i=0; i<len; ++i)
-		*p++ = *buffer;
+		*p++ = *buffer++;
 
 	status = I2C_do_buffer (0, I2C_MASTER_XMIT, chip, alen + len,
 				dummy_buffer, I2C_STOP, 1, I2C_NO_RESTART);
diff --git a/cpu/mpc824x/start.S b/cpu/mpc824x/start.S
index b1cdb8460d..a22137ceb6 100644
--- a/cpu/mpc824x/start.S
+++ b/cpu/mpc824x/start.S
@@ -259,74 +259,7 @@ ProgramCheck:
 	STD_EXCEPTION(EXC_OFF_DECR, Decrementer, timer_interrupt)
 	STD_EXCEPTION(0xa00, Trap_0a, UnknownException)
 	STD_EXCEPTION(0xb00, Trap_0b, UnknownException)
-
-	. = 0xc00
-/*
- * r0 - SYSCALL number
- * r3-... arguments
- */
-SystemCall:
-	addis	r11,r0,0		/* get functions table addr */
-	ori	r11,r11,0		/* Note: this code is patched in trap_init */
-	addis	r12,r0,0		/* get number of functions */
-	ori	r12,r12,0
-
-	cmplw	0, r0, r12
-	bge	1f
-
-	rlwinm	r0,r0,2,0,31		/* fn_addr = fn_tbl[r0] */
-	add	r11,r11,r0
-	lwz	r11,0(r11)
-
-	li	r20,0xd00-4		/* Get stack pointer */
-	lwz	r12,0(r20)
-	subi	r12,r12,12		/* Adjust stack pointer */
-	li	r0,0xc00+_end_back-SystemCall
-	cmplw	0, r0, r12		/* Check stack overflow */
-	bgt	1f
-	stw	r12,0(r20)
-
-	mflr	r0
-	stw	r0,0(r12)
-	mfspr	r0,SRR0
-	stw	r0,4(r12)
-	mfspr	r0,SRR1
-	stw	r0,8(r12)
-
-	li	r12,0xc00+_back-SystemCall
-	mtlr	r12
-	mtspr	SRR0,r11
-
-1:	SYNC
-	rfi
-
-_back:
-
-	mfmsr	r11			/* Disable interrupts */
-	li	r12,0
-	ori	r12,r12,MSR_EE
-	andc	r11,r11,r12
-	SYNC				/* Some chip revs need this... */
-	mtmsr	r11
-	SYNC
-
-	li	r12,0xd00-4		/* restore regs */
-	lwz	r12,0(r12)
-
-	lwz	r11,0(r12)
-	mtlr	r11
-	lwz	r11,4(r12)
-	mtspr	SRR0,r11
-	lwz	r11,8(r12)
-	mtspr	SRR1,r11
-
-	addi	r12,r12,12		/* Adjust stack pointer */
-	li	r20,0xd00-4
-	stw	r12,0(r20)
-
-	SYNC
-	rfi
-_end_back:
+	STD_EXCEPTION(0xc00, SystemCall, UnknownException)
 
 	STD_EXCEPTION(EXC_OFF_TRACE, SingleStep, UnknownException)