//
//  Low-level bootloader routines to replace "assembly.s90"
//  from the original ATMEL code. 
// 
//  See avr-libc's boot-module for more information
//  Thanks to Eric B. Weddington author of boot.h.
//
//  3/2004 Martin Thomas, Kaiserslautern, Germany
//
// todo: extend functions with espm

#include <avr/io.h>

// There are some #ifdefs in avr/boot.h V1.0 that do not "know"
// about the ATmega169. So some functions have been copied here 
// and small modifications have been done to avoid problems.

#include "chipdef.h"

/* Check for SPM Control Register in processor. */
#if defined (SPMCSR)
#  define SPM_REG    SPMCSR
#elif defined (SPMCR)
#  define SPM_REG    SPMCR
#else
#  error AVR processor does not provide bootloader support!
#endif

#define BOOT_PAGE_FILL                _BV(SPMEN)
#define BOOT_LOCK_BITS_SET           (_BV(SPMEN) | _BV(BLBSET))

#define boot_spm_busy()               (SPM_REG & (unsigned char)_BV(SPMEN))
#define boot_spm_busy_wait()          do{}while(boot_spm_busy())
#define eeprom_is_ready()             bit_is_clear(EECR, EEWE)
#define eeprom_is_ready_wait()         do{}while(!eeprom_is_ready())

// from avr/boot.h
// added "func" parameter and spm-busy check at end
#define _boot_page_write_alternate_bf(address,func)    \
({                                               \
    boot_spm_busy_wait();                        \
    eeprom_is_ready_wait();                      \
    asm volatile                                 \
    (                                            \
        "movw r30, %2\n\t"                       \
        "sts %0, %1\n\t"                         \
        "spm\n\t"                                \
        ".word 0xffff\n\t"                       \
        "nop\n\t"                                \
        : "=m" (SPM_REG)                       \
        : "r" ((unsigned char)func),             \
          "r" ((unsigned short)address)          \
        : "r30", "r31", "r0"                     \
    );                                           \
    boot_spm_busy_wait();                        \
})

// from avr/boot.h 
// added spm-busy check at end
#define _boot_page_fill_alternate_bf(address, data)\
({                                               \
    boot_spm_busy_wait();                        \
    eeprom_is_ready_wait();                      \
    asm volatile                                 \
    (                                            \
        "movw  r0, %3\n\t"                       \
        "movw r30, %2\n\t"                       \
        "sts %0, %1\n\t"                         \
        "spm\n\t"                                \
        ".word 0xffff\n\t"                       \
        "nop\n\t"                                \
        "clr  r1\n\t"                            \
        : "=m" (SPM_REG)                       \
        : "r" ((unsigned char)BOOT_PAGE_FILL), \
          "r" ((unsigned short)address),         \
          "r" ((unsigned short)data)             \
        : "r0", "r30", "r31"                     \
    );                                           \
    boot_spm_busy_wait();                        \
})

// from avr/boot.h
// added spm-busy check at end, removed lockbit-masking, 
// removed r30/r31 init - maybe wrong TODO: test this

#ifdef ENABLEREADFUSELOCK
#define _boot_lock_bits_set_alternate_bf(lock_bits)        \
({                                                         \
    boot_spm_busy_wait();                                  \
    eeprom_is_ready_wait();                                \
    asm volatile                                           \
    (                                                      \
        "mov r0, %2\n\t"                                   \
        "sts %0, %1\n\t"                                   \
        "spm\n\t"                                          \
        ".word 0xffff\n\t"                                 \
        "nop\n\t"                                          \
        : "=m" (SPM_REG)                                   \
        : "r" ((unsigned char)BOOT_LOCK_BITS_SET),         \
          "r" ((unsigned char) lock_bits)                  \
        : "r0"                                             \
    );                                                     \
    boot_spm_busy_wait();                                  \
})

void write_lock_bits (unsigned char val)
{
	_boot_lock_bits_set_alternate_bf(val);
}
#endif

void write_page (unsigned int adr, unsigned char function)
{
	_boot_page_write_alternate_bf(adr,function);
}

void fill_temp_buffer (unsigned int data,unsigned int adr)
{
	_boot_page_fill_alternate_bf(adr, data);
}

unsigned int read_program_memory(unsigned int adr ,unsigned char param)
// to read lockbits give param=0x09, if param!=0 it is written to SPMCSR
// this is a "translation" from the original code to gcc-inline-assembler
{
	unsigned int retval;
	
	boot_spm_busy_wait();
	eeprom_is_ready_wait();                      
	asm volatile
	(
		"movw r30, %3\n\t"
		"sbrc %2,0x00\n\t"
		"sts %0, %2\n\t"
		#ifdef LARGE_MEMORY  // If large memory (>64K) ELPM is needed to use RAMPZ
		"elpm\n\t"           // read LSB
		#else
		"lpm\n\t"            // R0 is now the LSB of the return value
		#endif
		"mov %A1,r0\n\t"
		"inc r30\n\t"
		#ifdef LARGE_MEMORY  // If large memory (>64K) ELPM is needed to use RAMPZ        
		"elpm\n\t"           // read MSB
		#else
		"lpm\n\t"            // R0 is now the MSB of the return value
		#endif
		"mov %B1,r0\n\t"  
		: "=m" (SPM_REG),
		  "=r" (retval)
		: "r" ((unsigned char)param),
		  "r" ((unsigned short)adr)
		: "r30", "r31", "r0"
	);
	return retval;
}