alt_u32 flash_write(T_flash_state *st, alt_u32 address, const alt_u8 *src_buffer, alt_u32 size, alt_u32 subsec_flag)
{
	alt_irq_context irq_previous_context;
	alt_u32 ret_flag = FLASH_SUCCESS;
	alt_u32 write_offset = address;
	alt_u32 remaining_length = size;
	alt_u32 buffer_offset = 0;
	alt_u32 flag_status;
	alt_u32 sec_size;
	alt_u32 sec_num;


	// If the size is 0 --> return
	if(size == 0)
		return FLASH_ERROR;

	if (subsec_flag == FLASH_WRITE_4K_SUBSECTOR)
	{
		sec_size = st->subsector_size;
		sec_num = st->number_of_subsectors;
	}
	else
	{
		sec_size = st->sector_size;
		sec_num = st->number_of_sectors;
	}

	// Check for aligned access
	if (address % sec_size != 0)
		return FLASH_UNALIGNED_ACCESS;


	alt_u32 lower_address_limit = 0;
	alt_u32 upper_address_limit = 0;
	alt_u32 sec_to_lock_address = 0;

	// Lock Flash with volatile reg
	lower_address_limit = address & ~(st->sector_size - 1);
	upper_address_limit = ((address + size) & ~(st->sector_size - 1));
	for (alt_u32 i = 0; i < st->number_of_sectors; i++)
	{
		if ((sec_to_lock_address < lower_address_limit) || (sec_to_lock_address > upper_address_limit))
		{
			oe_flash_write_volatile_lock_bit(st, sec_to_lock_address, 1);

			// Check for errors
			do {
				flag_status = oe_flash_read_flag_status_register(st);
			} while (!(flag_status & FLASH_FSR_READY));
			oe_flash_clear_flag_status_register(st);
			if (flag_status & FLASH_FSR_PROTECTION_ERR)
				return FLASH_SECTOR_PROTECTED;
			if ((flag_status & FLASH_FSR_ERASE_ERR) || (flag_status & FLASH_FSR_PROGRAM_ERR))
				return FLASH_ERROR;
		}

		// Next start address
		sec_to_lock_address += st->sector_size;
	}

	// Unlock Flash with non-volatile reg
	if (oe_flash_total_nonvolatile_unlock(st) != FLASH_SUCCESS)
		return FLASH_SECTOR_PROTECTED;


	// Inhibit all interrupts
	irq_previous_context = alt_irq_disable_all();

	for (alt_u32 i = address/sec_size; i < sec_num; i++)
	{
		alt_u32 block_offset = 0; // block offset in byte addressing
		alt_u32 offset_within_current_sector = 0; // offset into current sector to write
		alt_u32 length_to_write = 0; // length to write to current sector

		if (remaining_length <= 0)
		{
			break; // out of data to write
		}

		// calculate current sector/block offset in byte addressing
		block_offset = write_offset & ~(sec_size - 1);

		// calculate offset into sector/block if there is one
		if (block_offset != write_offset)
		{
			offset_within_current_sector = write_offset - block_offset;
		}

		// erase block
		if (subsec_flag == FLASH_WRITE_4K_SUBSECTOR)
		{
			ret_flag = oe_flash_4k_subsector_erase(st, block_offset);
		}
		else
		{
			ret_flag = oe_flash_sector_erase(st, block_offset);
		}

		if (ret_flag != FLASH_SUCCESS)
		{
			break;
		}

		// calculate the byte size of data to be written in a sector
		length_to_write = MIN(sec_size - offset_within_current_sector, remaining_length);

		// enable write operation
		oe_flash_write_enable(st);

		// write data
		memcpy((void*)(st->mem_base + write_offset), (void*)(src_buffer + buffer_offset), length_to_write);

		// check for errors
		do {
			flag_status = oe_flash_read_flag_status_register(st);
		} while (!(flag_status & FLASH_FSR_READY));
		oe_flash_clear_flag_status_register(st);
		if (flag_status & FLASH_FSR_PROTECTION_ERR)
		{
			ret_flag = FLASH_SECTOR_PROTECTED;
			break;
		}
		if ((flag_status & FLASH_FSR_ERASE_ERR) || (flag_status & FLASH_FSR_PROGRAM_ERR))
		{
			ret_flag = FLASH_ERROR;
			break;
		}

		// update remaining length and buffer_offset pointer
		remaining_length -= length_to_write;
		buffer_offset += length_to_write;
		write_offset += length_to_write;

		// call the external routine
		if (st->flash_cbk)
			st->flash_cbk();
	}

	// Enable all interrupts that were previously disabled by alt_irq_disable_all()
	alt_irq_enable_all(irq_previous_context);


	// Unlock Flash with volatile reg
	if (oe_flash_total_volatile_unlock(st) != FLASH_SUCCESS)
		return FLASH_ERROR;

	// Lock Flash with non-volatile reg
	if (oe_flash_total_nonvolatile_lock(st) != FLASH_SUCCESS)
		return FLASH_ERROR;

	return ret_flag;
}