diff --git a/assembly.c b/assembly.c
index 3025468..ec1b442 100644
--- a/assembly.c
+++ b/assembly.c
@@ -73,6 +73,7 @@
 //  +-------+----------------------------------+--------------+
 //  | 5'h15 | Call if carry flag is not set    |      NO      |
 //
+//
 //  |       |                                  |              |
 //  +-------+----------------------------------+--------------+
 //  | 5'h0F | MOV immediate to SP              |              |
@@ -82,20 +83,24 @@
 //
 //
 //  INSTRUCTION FORMAT 1 OPCODE NUM:
-//  +-------+----------------------------------+--------------+
-//  | NUM   | DESCRIPTION                      | AFFECT FLAGS |
-//  +-------+----------------------------------+--------------+
-//  | 13'd0 | ADD                              |      YES     |
-//  +-------+----------------------------------+--------------|
-//  | 13'd1 | SUBTRACT                         |      YES     |
-//  +-------+----------------------------------+--------------|
-//  | 13'd2 | SHIFT LEFT                       |      YES     |
-//  +-------+----------------------------------+--------------|
-//  | 13'd3 | SHIFT RIGHT                      |      YES     |
-//  +-------+----------------------------------+--------------|
-//  | 13'd4 | COMPARE  (SUBTRACT WITHOUT SAVE) |      YES     |
-//  +-------+----------------------------------+--------------+
-//  | 13'd5 | HALT                             |      NO      |
+//  +-------+----------------------------------+--------------+------------+------------+
+//  | NUM   | DESCRIPTION                      | AFFECT FLAGS | SOURCE REG |  DEST REG  |
+//  +-------+----------------------------------+--------------+------------+------------+
+//  | 13'd0 | ADD                              |      YES     |  operand 1 |  operand 1 |
+//  +-------+----------------------------------+--------------|------------+------------+
+//  | 13'd1 | SUBTRACT                         |      YES     |  operand 1 |  operand 1 |
+//  +-------+----------------------------------+--------------|------------+------------+
+//  | 13'd2 | SHIFT LEFT                       |      YES     |   operand  |            |
+//  +-------+----------------------------------+--------------|------------+------------+
+//  | 13'd3 | SHIFT RIGHT                      |      YES     |   operand  |            |
+//  +-------+----------------------------------+--------------|------------+------------+
+//  | 13'd4 | COMPARE  (SUBTRACT WITHOUT SAVE) |      YES     |  operand 1 |  operand 1 |
+//  +-------+----------------------------------+--------------+------------+------------+
+//  | 13'd5 | HALT                             |      NO      |            |            |
+//  +-------+----------------------------------+--------------+------------+------------+
+//  | 13'd6 | PUSH                             |      NO      |   operand  |            |
+//  +-------+----------------------------------+--------------+------------+------------+
+//  | 13'd7 | POP                              |      NO      |   operand  |            |
 //
 //
 //  INSTRUCTION FORMAT 2 OPCODE NUM:
@@ -183,6 +188,12 @@ char *disassemble(uint32_t opcode_be){
 			case 0x05:
 				snprintf(ret,MAX_INSTRUCTION_LENGTH,"HALT");
 				break;
+			case 0x06:
+				snprintf(ret,MAX_INSTRUCTION_LENGTH,"PUSH %%R%0d",val1);
+				break;
+			case 0x07:
+				snprintf(ret,MAX_INSTRUCTION_LENGTH,"POP %%R%0d",val1);
+				break;
 			default:
 				snprintf(ret,MAX_INSTRUCTION_LENGTH,"UNRECOGNISED INSTRUCTION");
 				break;
@@ -407,8 +418,37 @@ uint32_t assemble_line(char *line, struct assembler_context_t *assembler_context
 				return 0xFFFFFFFF;
 		}else if(strncmp(line,"RET",3)==0){
 			return 0x10000000;
-		}else if(*line==':')
+		}else if(*line==':'){
 			return 0xFFFFFFF0;
+		}else if(strncmp(line,"PUSH",4)==0){
+			x=4;
+			while(line[x]==' ')x++;
+			if(line[x]=='%'){
+				x++;
+				if(line[x]=='R'){
+					x++;
+					if(line[x]>='0'&&line[x]<='7')
+						return 0x20060000|(line[x]-'0')<<8;
+					else
+						return 0xFFFFFFFF;
+				}
+			}else
+				return 0xFFFFFFFF;
+		}else if(strncmp(line,"POP",3)==0){
+			x=3;
+			while(line[x]==' ')x++;
+			if(line[x]=='%'){
+				x++;
+				if(line[x]=='R'){
+					x++;
+					if(line[x]>='0'&&line[x]<='7')
+						return 0x20070000|(line[x]-'0')<<8;
+					else
+						return 0xFFFFFFFF;
+				}
+			}else
+				return 0xFFFFFFFF;
+		}
 		return 0xFFFFFFFF;
 	}
 }
diff --git a/cpu.c b/cpu.c
index 0c200b6..2371c08 100644
--- a/cpu.c
+++ b/cpu.c
@@ -86,9 +86,15 @@ int decode(struct simdata_t *simdata){
 			simdata->exec_data->in_op2->data=op2;
 
 			switch(opcode){
-				case 5:
+				case 0x05:
 					simdata->exec_data->EXEC_ACTION=HALT;
 					break;
+				case 0x06:
+					simdata->exec_data->EXEC_ACTION=PUSH;
+					break;
+				case 0x07:
+					simdata->exec_data->EXEC_ACTION=POP;
+					break;
 				default:
 					simdata->exec_data->EXEC_ACTION=EXEC_ALU;
 					break;
@@ -116,6 +122,8 @@ int decode(struct simdata_t *simdata){
 				case 3: simdata->exec_data->ALU_OP=ALU_SR;  break;
 				case 4: simdata->exec_data->ALU_OP=ALU_CMP;  break;
 				case 5: break;
+				case 6: break;
+				case 7: break;
 				default:
 					return 1;
 			}
@@ -200,17 +208,17 @@ int exec(struct simdata_t *simdata){
 			}
 			if(condition){
 				if(simdata->exec_data->EXEC_ACTION==CALL){
-					simdata->RAM[simdata->registers->SP]=simdata->registers->PC;
-					simdata->registers->SP+=6;
+					*(uint32_t*)(simdata->RAM+simdata->registers->SP)=simdata->registers->PC;
+					simdata->registers->SP+=4;
 				}
 				if(simdata->exec_data->out_op->OP_ADDR==IMMEDIATE)
-					simdata->registers->PC=simdata->exec_data->out_op->data;
+					simdata->registers->PC=(uint32_t)(simdata->exec_data->out_op->data);
 				else
 					return 1;
 			}
 			break;
 		case RET:
-			simdata->registers->SP-=6;
+			simdata->registers->SP-=4;
 			simdata->registers->PC=simdata->RAM[simdata->registers->SP];
 			break;
 		case EXEC_ALU:
@@ -290,7 +298,18 @@ int exec(struct simdata_t *simdata){
 		case HALT:
 			  simdata->cpu_state=CPU_HALTED;
 			  break;
-
+		case PUSH:
+			  simdata->RAM[simdata->registers->SP  ]=simdata->registers->GPR[simdata->exec_data->in_op1->data]&0x000000FF;
+			  simdata->RAM[simdata->registers->SP+1]=(simdata->registers->GPR[simdata->exec_data->in_op1->data]&0x0000FF00)>>8;
+			  simdata->RAM[simdata->registers->SP+2]=(simdata->registers->GPR[simdata->exec_data->in_op1->data]&0x00FF0000)>>16;
+			  simdata->registers->SP+=4;
+			  break;
+		case POP:
+			  simdata->registers->SP-=4;
+			  simdata->registers->GPR[simdata->exec_data->in_op1->data]=simdata->RAM[simdata->registers->SP];
+			  simdata->registers->GPR[simdata->exec_data->in_op1->data]|=simdata->RAM[simdata->registers->SP+1]<<8;
+			  simdata->registers->GPR[simdata->exec_data->in_op1->data]|=simdata->RAM[simdata->registers->SP+2]<<16;
+			  break;
 	}
 	add_to_instr_list(&simdata->cpu_gui_hints->executing_list,simdata->exec_data->address);
 	return 0;
diff --git a/cpu.h b/cpu.h
index ccde7fc..4659315 100644
--- a/cpu.h
+++ b/cpu.h
@@ -20,7 +20,9 @@ enum EXEC_ACTION_t {
 	CALL,
 	NOP,
 	HALT,
-	RET
+	RET,
+	PUSH,
+	POP
 };
 
 enum ALU_OP_t {
diff --git a/test.asm b/test.asm
index db53da7..691e1e8 100644
--- a/test.asm
+++ b/test.asm
@@ -1,13 +1,19 @@
-MOV $123456,%SP
+MOV $FF0000,%SP
 MOV $0x0001,%R0l
 MOV $0x0000,%R0h
-MOV $0x0000,%R1l
+MOV $0xBEEF,%R1l
 MOV $0x0000,%R1h
 CALL $SUBROUTINE
 CALL $SUBROUTINE
 CALL $SUBROUTINE
+CALL $SUBROUTINE2
 JMP $END
 :SUBROUTINE
+PUSH %R1
+ADD %R0,%R1
+POP %R1
+RET
+:SUBROUTINE2
 ADD %R0,%R1
 RET
 :END