	/* GNU compatible definitions */
	.global	fill_temp_buffer
	.global	write_page
	.global	write_lock_bits
	.global read_program_memory

	/* The definitions of registers used in the AVRtools env
	 * assume that sts/lds are used, not IN/OUT (ie. they
	 * add 0x20 to the address
	 */
	#define OUT	sts
	#define IN	lds


	/* The IAR compiler originally used has a different ABI
	 * to the GNU C assembler used here
	 */
	#define	R16	r24
	#define	R17	r25
	#define R18	r22
	#define	R19	r23

	/* Define a 16 bit word */
	#define DW	.short
	
	.section	.text

  #include "defines.h"
  #define _SPM    DW 0x95E8 ;Workaround for not having enhanced core Assembler in EWAAVR 2.25


write_page:
        RCALL   WAIT_SPMEN  ;Wait for SPMEN flag cleared
        MOV     R31,R17         
        MOV     R30,R16     ;move adress to z pointer (R31=ZH R30=ZL)
        LOAD_SPM_CONTROL_REGISTER_MACRO   ; argument 2 decides function (r18)
        _SPM                ;Store program memory
        DW      0xFFFF      ;For future part compatibility, instruction alignment
        NOP
        RJMP    WAIT_SPMEN ;Wait for SPMEN flag cleared


fill_temp_buffer:
        RCALL   WAIT_SPMEN  ;Wait for SPMEN flag cleared
        MOV     R31,R19     ;move adress to z pointer (R31=ZH R30=ZL)   
        MOV     R30,R18     
        MOV     R1,R17      ;move data to reg 0 and 1      
        MOV     R0,R16
        
        LDI     R18,(1<<SPMEN)
        LOAD_SPM_CONTROL_REGISTER_MACRO   ; r18 decides function
        _SPM                ;Store program memory
        DW      0xFFFF      ;For future part compatibility, instruction alignment
        NOP
        RJMP   WAIT_SPMEN  ;Wait for SPMEN flag cleared


read_program_memory:
        RCALL    WAIT_SPMEN
        MOV     R31,R17     ;R31=ZH R30=ZL
        MOV     R30,R16     ;move adress to z pointer
        SBRC    R18,0       ;read lockbits? (second argument=0x09)
                            ;if so, place second argument in SPMEN register
        LOAD_SPM_CONTROL_REGISTER_MACRO   ; r18 decides function
        
        LPM                 

        MOV     R16,R0      ;read LSB           
        INC     R30

        LPM                 

        MOV     R17,R0      ;read MSB (ignored when reading lockbits)
        RET


write_lock_bits:
        RCALL   WAIT_SPMEN  ;Wait for SPMEN flag cleared
        MOV     R0,R16   
        LDI     R18,((1<<BLBSET)|(1<<SPMEN))

        LOAD_SPM_CONTROL_REGISTER_MACRO   ; r18 decides function
        _SPM                ;write lockbits
        DW      0xFFFF      ;For future part compatibility, instruction alignment
        NOP
        
        RJMP   WAIT_SPMEN  ;Wait for SPMEN flag cleared
        

WAIT_SPMEN:
		MOV		R0, R18
        GET_SPM_CONTROL_REGISTER_MACRO  ; get SPMCR into r18
        SBRC    R18,SPMEN
        RJMP    WAIT_SPMEN  ;Wait for SPMEN flag cleared
        MOV		R18, R0

        RET
