Index: cpukit/score/cpu/avr/ChangeLog
===================================================================
--- cpukit/score/cpu/avr/ChangeLog	(revision 79d490aa58e4ee2409c6033c771d276029ee7d6f)
+++ cpukit/score/cpu/avr/ChangeLog	(revision cdfe85a7f3c29489a8f7f8848e4d351eaf4d0b76)
@@ -1,2 +1,7 @@
+2009-08-11	Josh Switnicki <josh.switnicki@utoronto.ca>
+
+	* cpu.c, cpu_asm.S, rtems/score/cpu.h: Fix bug in
+	_CPU_Context_Initialize.
+
 2009-08-05	Josh Switnicki <josh.switnicki@utoronto.ca>
 
Index: cpukit/score/cpu/avr/cpu.c
===================================================================
--- cpukit/score/cpu/avr/cpu.c	(revision 79d490aa58e4ee2409c6033c771d276029ee7d6f)
+++ cpukit/score/cpu/avr/cpu.c	(revision cdfe85a7f3c29489a8f7f8848e4d351eaf4d0b76)
@@ -31,6 +31,5 @@
 void _CPU_Initialize(void)
 {
-  printk( "AVR CPU Initialize\n" );
-
+ 
   /*
    *  If there is not an easy way to initialize the FP context
@@ -41,80 +40,4 @@
 
   /* FP context initialization support goes here */
-}
-
-/*PAGE
- *
- *  _CPU_Context_Initialize
- *
- *  This kernel routine initializes the basic non-FP context area associated
- *  with each thread.
- *
- *  Input parameters:
- *    the_context  - pointer to the context area
- *    stack_base   - address of memory for the SPARC
- *    size         - size in bytes of the stack area
- *    new_level    - interrupt level for this context area
- *    entry_point  - the starting execution point for this this context
- *    is_fp        - TRUE if this context is associated with an FP thread
- *
- *  Output parameters: NONE
- */
-
-void _CPU_Context_Initialize(
-  Context_Control  *the_context,
-  uint32_t         *stack_base,
-  uint32_t          size,
-  uint32_t          new_level,
-  void             *entry_point,
-  bool              is_fp
-)
-{
-  uint16_t stack;
-  uint16_t start;
-  uint8_t  *tmp_sp;
-  uint8_t   start_low;
-  uint8_t   start_high;
-
-  /* calc stack high end */
-  stack = (uint16_t) (stack_base) + (uint16_t) (size);
-  /* calc stack pointer initial value */
-  the_context->stack_pointer = (stack - 18);
-  /* put the entry point on the stack */
-  start = (uint16_t) entry_point;
-  start_low = start & 0xff;
-  start_high = start >> 8;
-  tmp_sp = (uint8_t *) (stack+1);
-  tmp_sp[0] = start_high;
-  tmp_sp[1] = start_low;
-
-  if (new_level) the_context->status = 0x00; //interrupts are enabled
-  else           the_context->status = 0x80; //interrupts are disabled
-
-  /*
-   * JOEL: Why if I comment out these three lines does ticker not run?
-   */
-#if 1
-  printk( "tmp_sp=0x%04x ", ((uint16_t)tmp_sp) );
-  printk("the_context = 0x%x\n", the_context);
-  printk("entry = 0x%x\n", entry_point);
-#endif
-#if 0
-  printk("sp = 0x%x\n\n",stack);
-
- { int i; uint8_t *p;
-   p = (uint8_t *)(stack - 18);
-   for( i=0 ; i<=20 ; i+=8 ) {
-     printk( "0x%04x ", ((uint16_t)&p[i]) );
-     printk(
-      "0x%02x 0x%02x 0x%02x 0x%02x   ",
-      p[i + 0], p[i + 1], p[i + 2], p[i + 3]
-     );
-     printk(
-      "0x%02x 0x%02x 0x%02x 0x%02x\n",
-      p[i + 4], p[i + 5], p[i + 6], p[i + 7]
-     );
-   }
- }
-#endif
 }
 
@@ -133,6 +56,7 @@
    *  This routine returns the current interrupt level.
    */
+  if((SREG & 0x80))return 1;
+  else 	return 0;
 
-  return 0;
 }
 
Index: cpukit/score/cpu/avr/cpu_asm.S
===================================================================
--- cpukit/score/cpu/avr/cpu_asm.S	(revision 79d490aa58e4ee2409c6033c771d276029ee7d6f)
+++ cpukit/score/cpu/avr/cpu_asm.S	(revision cdfe85a7f3c29489a8f7f8848e4d351eaf4d0b76)
@@ -209,9 +209,19 @@
 	PUBLIC(_CPU_Context_switch)
 SYM(_CPU_Context_switch):
-	mov	r26, r24 	/* r26,r27 is X or current context */
+	mov	r26, r24 	/*r26,r27 is X*/
 	mov	r27, r25
-	mov	r24, r22        /* r24,r25 is heir context */
+	mov	r24, r22
  	mov	r25, r23
 	/*save registers*/
+#if 1
+/*if this section is removed there is a problem.*/
+/*debug section start*/ 
+	pop	r22
+	pop	r23
+	push	r22
+	push	r23
+/*debug section end*/
+#endif
+
 	push	r2
 	push	r3
@@ -230,20 +240,21 @@
 	push	r16
 	push	r17
+
 	push	r28
 	push	r29
+
 	/*load sreg*/
-	lds 	r23,0x5F  /*load sreg*/
+	lds 	r23,0x5f  /*load sreg*/
 	/*disable interrupts*/
 	cli
 	/*load stack pointer*/
-	lds	r22,0x5D /*spl*/
-	lds 	r21,0x5E /*sph*/
-	/*save sp to context struct*/
+	lds	r22,0x5d /*spl*/
+	lds 	r21,0x5e /*sph*/
+	/*save sreg and sp to context struct*/
+
 	/*save low then high byte --- verify this delete when verified*/
 	st	X+, r22
 	st	X+, r21
-	/*save sreg and sp to context struct*/
 	st 	X, r23
-
 
 	PUBLIC(_CPU_Context_restore)
@@ -252,12 +263,17 @@
 	mov	r26,r24         /* R26/27 are X */
 	mov	r27,r25
+
 	/*restore stack pointer*/
 	ld	r25, X+
 	ld	r24, X+	
-	sts	0x5E, r24 /*sph*/
-	sts 	0x5D, r25 /*spl*/
+	sts	0x5E,r24  /*sph*/
+	sts 	0x5D ,r25 /*spl*/
 	/*restore registers from stack*/
+
+
 	pop	r29
 	pop	r28
+
+
 	pop	r17
 	pop	r16
@@ -276,39 +292,90 @@
 	pop	r3
 	pop	r2
-	/*restore status register*/
-	ld	r23, X
-	sts	0x5f,r23 /*sreg*/	
+
+	/*restore sreg*/
+	ld	r25, X
+	sts	0x5f,r25 /*sreg*/
+	pop	r30
+	pop	r31
+	IJMP
 	ret
 
 
 
-/*
- *  _CPU_Context_restore
- *
- *  This routine is generally used only to restart self in an
- *  efficient manner.  It may simply be a label in _CPU_Context_switch.
- *
- *  NOTE: May be unnecessary to reload some registers.
- *
- *  NO_CPU Specific Information:
- *
- *  XXX document implementation including references if appropriate
- 
-
-void _CPU_Context_restore(
-  Context_Control *new_context
-)
-{
-  printk( "AVR _CPU_Context_restore\n" );
-}
-*/
-/*
-	PUBLIC(_CPU_Context_restore)
-
-SYM(_CPU_Context_restore):
-	//call printk("AVR _CPU_Context_restore\n")
-	ret
-
-*/
+/*PAGE
+ *
+ *  _CPU_Context_Initialize
+ *
+ *  This kernel routine initializes the basic non-FP context area associated
+ *  with each thread.
+ *
+ *  Input parameters:
+ *    the_context  - pointer to the context area
+ *    stack_base   - address of memory for the SPARC
+ *    size         - size in bytes of the stack area
+ *    new_level    - interrupt level for this context area
+ *    entry_point  - the starting execution point for this this context
+ *    is_fp        - TRUE if this context is associated with an FP thread
+ *
+ *	the_context is in r25,r24
+ *	entry point is in r13,r12
+ *	stack base is in r23, r22
+ *	size is in r21, r20, r19,r18
+ *	newleve is in r14,r15, r16, 17
+ *
+ *
+ *  Output parameters: NONE
+ */
+
+	PUBLIC(_CPU_Context_Initialize)
+SYM(_CPU_Context_Initialize):
+	//save caller saved regs
+	PUSH	R10
+	PUSH 	R11
+	PUSH	R12
+	PUSH 	R13
+	PUSH 	R14
+	PUSH 	R15
+	PUSH 	R16
+	PUSH	R17
+	PUSH	R28
+	PUSH	R29
+	//calculate new stack pointer 
+	ADD	R22, R18
+	ADC	R23, R19
+	MOV	R26, R22
+	MOV	R27, R23
+	//Initialize stack with entry point
+	ST	-X, R13
+	ST	-X, R12
+	//store new stack pointer in context control
+	SBIW	R26, 0X13	/*subtract 33 to account for registers*/
+	MOV	R28, R24
+	MOV	R29, R25
+	STD	Y+1, R27	//save stack pointer high to context control
+	ST	Y, R26		//save stack pointer low to context control
+	//set interrupt level in new context
+	LDI	R18, 0		//set sreg in new context to zero
+	STD	Y+2, R18 	//interrupts not enabled		
+	MOV	R18, R14
+	CPI	R18, 0
+	BRNE	NEW_LEVEL_ZERO
+	LDI	R18, 0X80	//set sreg in new context to 0x80
+	STD	Y+2, R18	//interupts enabled
+NEW_LEVEL_ZERO:
+	//restore caller saved regs
+	POP	R29
+	POP	R28
+	POP	R17
+	POP	R16
+	POP	R15
+	POP	R14
+	POP	R13
+	POP	R12
+	POP	R11
+	POP	R10
+	RET
+	
+
 
 /*  void __ISR_Handler()
Index: cpukit/score/cpu/avr/rtems/score/cpu.h
===================================================================
--- cpukit/score/cpu/avr/rtems/score/cpu.h	(revision 79d490aa58e4ee2409c6033c771d276029ee7d6f)
+++ cpukit/score/cpu/avr/rtems/score/cpu.h	(revision cdfe85a7f3c29489a8f7f8848e4d351eaf4d0b76)
@@ -763,5 +763,5 @@
 
 #define _CPU_Context_Restart_self( _the_context ) \
-   _CPU_Context_restore( (_the_context) );
+   _CPU_Context_restore( _the_context );
 
 /*
