at91sam3.c

/***************************************************************************
 *   Copyright (C) 2009 by Duane Ellis                                     *
 *   openocd@duaneellis.com                                                *
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS for A PARTICULAR PURPOSE.  See the         *
 *   GNU General public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
****************************************************************************/

/* Some of the the lower level code was based on code supplied by
 * ATMEL under this copyright. */

/* BEGIN ATMEL COPYRIGHT */
/* ----------------------------------------------------------------------------
 *         ATMEL Microcontroller Software Support
 * ----------------------------------------------------------------------------
 * Copyright (c) 2009, Atmel Corporation
 *
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * - Redistributions of source code must retain the above copyright notice,
 * this list of conditions and the disclaimer below.
 *
 * Atmel's name may not be used to endorse or promote products derived from
 * this software without specific prior written permission.
 *
 * DISCLAIMER: THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR
 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT ARE
 * DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
 * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
 * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 * ----------------------------------------------------------------------------
 */
/* END ATMEL COPYRIGHT */

#ifdef HAVE_CONFIG_H
#include "config.h"
#endif


#include <stdio.h>
#include <string.h>
#include <stddef.h>
#include "log.h"
#include "types.h"
#include "flash.h"
#include "target.h"
#include "membuf.h"
#include "at91sam3.h"
#include "time_support.h"

#define REG_NAME_WIDTH  (12)


#define FLASH_BANK0_BASE   0x00080000
#define FLASH_BANK1_BASE   0x00100000

#define 	AT91C_EFC_FCMD_GETD                 (0x0) // (EFC) Get Flash Descriptor
#define 	AT91C_EFC_FCMD_WP                   (0x1) // (EFC) Write Page
#define 	AT91C_EFC_FCMD_WPL                  (0x2) // (EFC) Write Page and Lock
#define 	AT91C_EFC_FCMD_EWP                  (0x3) // (EFC) Erase Page and Write Page
#define 	AT91C_EFC_FCMD_EWPL                 (0x4) // (EFC) Erase Page and Write Page then Lock
#define 	AT91C_EFC_FCMD_EA                   (0x5) // (EFC) Erase All
// cmd6 is not present int he at91sam3u4/2/1 data sheet table 17-2
// #define 	AT91C_EFC_FCMD_EPL                  (0x6) // (EFC) Erase plane?
// cmd7 is not present int he at91sam3u4/2/1 data sheet table 17-2
// #define 	AT91C_EFC_FCMD_EPA                  (0x7) // (EFC) Erase pages?
#define 	AT91C_EFC_FCMD_SLB                  (0x8) // (EFC) Set Lock Bit
#define 	AT91C_EFC_FCMD_CLB                  (0x9) // (EFC) Clear Lock Bit
#define 	AT91C_EFC_FCMD_GLB                  (0xA) // (EFC) Get Lock Bit
#define 	AT91C_EFC_FCMD_SFB                  (0xB) // (EFC) Set Fuse Bit
#define 	AT91C_EFC_FCMD_CFB                  (0xC) // (EFC) Clear Fuse Bit
#define 	AT91C_EFC_FCMD_GFB                  (0xD) // (EFC) Get Fuse Bit
#define 	AT91C_EFC_FCMD_STUI                 (0xE) // (EFC) Start Read Unique ID
#define 	AT91C_EFC_FCMD_SPUI                 (0xF) // (EFC) Stop Read Unique ID

#define  offset_EFC_FMR   0
#define  offset_EFC_FCR   4
#define  offset_EFC_FSR   8
#define  offset_EFC_FRR   12


static float
_tomhz(uint32_t freq_hz)
{
	float f;

	f = ((float)(freq_hz)) / 1000000.0;
	return f;
}

// How the chip is configured.
struct sam3_cfg {
	uint32_t unique_id[4];

	uint32_t slow_freq;
	uint32_t rc_freq;
	uint32_t mainosc_freq;
	uint32_t plla_freq;
	uint32_t mclk_freq;
	uint32_t cpu_freq;
	uint32_t fclk_freq;
	uint32_t pclk0_freq;
	uint32_t pclk1_freq;
	uint32_t pclk2_freq;


#define SAM3_CHIPID_CIDR          (0x400E0740)
	uint32_t CHIPID_CIDR;
#define SAM3_CHIPID_EXID          (0x400E0744)
	uint32_t CHIPID_EXID;

#define SAM3_SUPC_CR              (0x400E1210)
	uint32_t SUPC_CR;

#define SAM3_PMC_BASE             (0x400E0400)
#define SAM3_PMC_SCSR             (SAM3_PMC_BASE + 0x0008)
	uint32_t PMC_SCSR;
#define SAM3_PMC_PCSR             (SAM3_PMC_BASE + 0x0018)
	uint32_t PMC_PCSR;
#define SAM3_CKGR_UCKR            (SAM3_PMC_BASE + 0x001c)
	uint32_t CKGR_UCKR;
#define SAM3_CKGR_MOR             (SAM3_PMC_BASE + 0x0020)
	uint32_t CKGR_MOR;
#define SAM3_CKGR_MCFR            (SAM3_PMC_BASE + 0x0024)
	uint32_t CKGR_MCFR;
#define SAM3_CKGR_PLLAR           (SAM3_PMC_BASE + 0x0028)
	uint32_t CKGR_PLLAR;
#define SAM3_PMC_MCKR             (SAM3_PMC_BASE + 0x0030)
	uint32_t PMC_MCKR;
#define SAM3_PMC_PCK0             (SAM3_PMC_BASE + 0x0040)
	uint32_t PMC_PCK0;
#define SAM3_PMC_PCK1             (SAM3_PMC_BASE + 0x0044)
	uint32_t PMC_PCK1;
#define SAM3_PMC_PCK2             (SAM3_PMC_BASE + 0x0048)
	uint32_t PMC_PCK2;
#define SAM3_PMC_SR               (SAM3_PMC_BASE + 0x0068)
	uint32_t PMC_SR;
#define SAM3_PMC_IMR              (SAM3_PMC_BASE + 0x006c)
	uint32_t PMC_IMR;
#define SAM3_PMC_FSMR             (SAM3_PMC_BASE + 0x0070)
	uint32_t PMC_FSMR;
#define SAM3_PMC_FSPR             (SAM3_PMC_BASE + 0x0074)
	uint32_t PMC_FSPR;
};


struct sam3_bank_private {
	int probed;
	// DANGER: THERE ARE DRAGONS HERE..
	// NOTE: If you add more 'ghost' pointers
	// be aware that you must *manually* update
	// these pointers in the function sam3_GetDetails()
	// See the comment "Here there be dragons"

	// so we can find the chip we belong to
	struct sam3_chip *pChip;
	// so we can find the orginal bank pointer
	flash_bank_t *pBank;
	unsigned bank_number;
	uint32_t controller_address;
	uint32_t base_address;
	bool present;
	unsigned size_bytes;
	unsigned nsectors;
	unsigned sector_size;
	unsigned page_size;
};

struct sam3_chip_details {
	// THERE ARE DRAGONS HERE..
	// note: If you add pointers here
	// becareful about them as they
	// may need to be updated inside
	// the function: "sam3_GetDetails()
	// which copy/overwrites the
	// 'runtime' copy of this structure
	uint32_t chipid_cidr;
	const char *name;

	unsigned n_gpnvms;
#define SAM3_N_NVM_BITS 3
	unsigned  gpnvm[SAM3_N_NVM_BITS];
	unsigned  total_flash_size;
	unsigned  total_sram_size;
	unsigned  n_banks;
#define SAM3_MAX_FLASH_BANKS 2
	// these are "initialized" from the global const data
	struct sam3_bank_private bank[SAM3_MAX_FLASH_BANKS];
};


struct sam3_chip {
	struct sam3_chip *next;
	int    probed;

	// this is "initialized" from the global const structure
	struct sam3_chip_details details;
	target_t *target;
	struct sam3_cfg cfg;

	struct membuf *mbuf;
};


struct sam3_reg_list {
	uint32_t address;  size_t struct_offset; const char *name;
	void (*explain_func)(struct sam3_chip *pInfo);
};


static struct sam3_chip *all_sam3_chips;

static struct sam3_chip *
get_current_sam3(struct command_context_s *cmd_ctx)
{
	target_t *t;
	static struct sam3_chip *p;

	t = get_current_target(cmd_ctx);
	if (!t) {
		command_print(cmd_ctx, "No current target?");
		return NULL;
	}

	p = all_sam3_chips;
	if (!p) {
		// this should not happen
		// the command is not registered until the chip is created?
		command_print(cmd_ctx, "No SAM3 chips exist?");
		return NULL;
	}

	while (p) {
		if (p->target == t) {
			return p;
		}
		p = p->next;
	}
	command_print(cmd_ctx, "Cannot find SAM3 chip?");
	return NULL;
}


// these are used to *initialize* the "pChip->details" structure.
static const struct sam3_chip_details all_sam3_details[] = {
	{
		.chipid_cidr    = 0x28100960,
		.name           = "at91sam3u4e",
		.total_flash_size     = 256 * 1024,
		.total_sram_size      = 52 * 1024,
		.n_gpnvms       = 3,
		.n_banks        = 2,

		// System boots at address 0x0
		// gpnvm[1] = selects boot code
		//     if gpnvm[1] == 0
		//         boot is via "SAMBA" (rom)
		//     else
		//         boot is via FLASH
		//         Selection is via gpnvm[2]
		//     endif
		//
		// NOTE: banks 0 & 1 switch places
		//     if gpnvm[2] == 0
		//         Bank0 is the boot rom
		//      else
		//         Bank1 is the boot rom
		//      endif
//		.bank[0] = {
		{
		  {
			.probed = 0,
			.pChip  = NULL,
			.pBank  = NULL,
			.bank_number = 0,
			.base_address = FLASH_BANK0_BASE,
			.controller_address = 0x400e0800,
			.present = 1,
			.size_bytes = 128 * 1024,
			.nsectors   = 16,
			.sector_size = 8192,
			.page_size   = 256,
		  },

//		.bank[1] = {
		  {
			.probed = 0,
			.pChip  = NULL,
			.pBank  = NULL,
			.bank_number = 1,
			.base_address = FLASH_BANK1_BASE,
			.controller_address = 0x400e0a00,
			.present = 1,
			.size_bytes = 128 * 1024,
			.nsectors   = 16,
			.sector_size = 8192,
			.page_size   = 256,
		  },
		},
	},

	{
		.chipid_cidr    = 0x281a0760,
		.name           = "at91sam3u2e",
		.total_flash_size     = 128 * 1024,
		.total_sram_size      =  36 * 1024,
		.n_gpnvms       = 2,
		.n_banks        = 1,

		// System boots at address 0x0
		// gpnvm[1] = selects boot code
		//     if gpnvm[1] == 0
		//         boot is via "SAMBA" (rom)
		//     else
		//         boot is via FLASH
		//         Selection is via gpnvm[2]
		//     endif
//		.bank[0] = {
		{
		  {
			.probed = 0,
			.pChip  = NULL,
			.pBank  = NULL,
			.bank_number = 0,
			.base_address = FLASH_BANK0_BASE,
			.controller_address = 0x400e0800,
			.present = 1,
			.size_bytes = 128 * 1024,
			.nsectors   = 16,
			.sector_size = 8192,
			.page_size   = 256,
		  },
//		  .bank[1] = {
		  {
			.present = 0,
			.probed = 0,
			.bank_number = 1,
		  },
		},
	},
	{
		.chipid_cidr    = 0x28190560,
		.name           = "at91sam3u1e",
		.total_flash_size     = 64 * 1024,
		.total_sram_size      = 20 * 1024,
		.n_gpnvms       = 2,
		.n_banks        = 1,

		// System boots at address 0x0
		// gpnvm[1] = selects boot code
		//     if gpnvm[1] == 0
		//         boot is via "SAMBA" (rom)
		//     else
		//         boot is via FLASH
		//         Selection is via gpnvm[2]
		//     endif
		//

//		.bank[0] = {
		{
		  {
			.probed = 0,
			.pChip  = NULL,
			.pBank  = NULL,
			.bank_number = 0,
			.base_address = FLASH_BANK0_BASE,
			.controller_address = 0x400e0800,
			.present = 1,
			.size_bytes =  64 * 1024,
			.nsectors   =  8,
			.sector_size = 8192,
			.page_size   = 256,
		  },

//		.bank[1] = {
		  {
			.present = 0,
			.probed = 0,
			.bank_number = 1,
		  },
		},
	},

	{
		.chipid_cidr    = 0x28000960,
		.name           = "at91sam3u4c",
		.total_flash_size     = 256 * 1024,
		.total_sram_size      = 52 * 1024,
		.n_gpnvms       = 3,
		.n_banks        = 2,

		// System boots at address 0x0
		// gpnvm[1] = selects boot code
		//     if gpnvm[1] == 0
		//         boot is via "SAMBA" (rom)
		//     else
		//         boot is via FLASH
		//         Selection is via gpnvm[2]
		//     endif
		//
		// NOTE: banks 0 & 1 switch places
		//     if gpnvm[2] == 0
		//         Bank0 is the boot rom
		//      else
		//         Bank1 is the boot rom
		//      endif
		{
		  {
//		.bank[0] = {
			.probed = 0,
			.pChip  = NULL,
			.pBank  = NULL,
			.bank_number = 0,
			.base_address = FLASH_BANK0_BASE,
			.controller_address = 0x400e0800,
			.present = 1,
			.size_bytes = 128 * 1024,
			.nsectors   = 16,
			.sector_size = 8192,
			.page_size   = 256,
		  },
//		.bank[1] = {
		  {
			.probed = 0,
			.pChip  = NULL,
			.pBank  = NULL,
			.bank_number = 1,
			.base_address = FLASH_BANK1_BASE,
			.controller_address = 0x400e0a00,
			.present = 1,
			.size_bytes = 128 * 1024,
			.nsectors   = 16,
			.sector_size = 8192,
			.page_size   = 256,
		  },
		},
	},

	{
		.chipid_cidr    = 0x280a0760,
		.name           = "at91sam3u2c",
		.total_flash_size     = 128 * 1024,
		.total_sram_size      = 36 * 1024,
		.n_gpnvms       = 2,
		.n_banks        = 1,

		// System boots at address 0x0
		// gpnvm[1] = selects boot code
		//     if gpnvm[1] == 0
		//         boot is via "SAMBA" (rom)
		//     else
		//         boot is via FLASH
		//         Selection is via gpnvm[2]
		//     endif
		{
//		.bank[0] = {
		  {
			.probed = 0,
			.pChip  = NULL,
			.pBank  = NULL,
			.bank_number = 0,
			.base_address = FLASH_BANK0_BASE,
			.controller_address = 0x400e0800,
			.present = 1,
			.size_bytes = 128 * 1024,
			.nsectors   = 16,
			.sector_size = 8192,
			.page_size   = 256,
		  },
//		.bank[1] = {
		  {
			.present = 0,
			.probed = 0,
			.bank_number = 1,
		  },
		},
	},
	{
		.chipid_cidr    = 0x28090560,
		.name           = "at91sam3u1c",
		.total_flash_size     = 64 * 1024,
		.total_sram_size      = 20 * 1024,
		.n_gpnvms       = 2,
		.n_banks        = 1,

		// System boots at address 0x0
		// gpnvm[1] = selects boot code
		//     if gpnvm[1] == 0
		//         boot is via "SAMBA" (rom)
		//     else
		//         boot is via FLASH
		//         Selection is via gpnvm[2]
		//     endif
		//

		{
//		.bank[0] = {
		  {
			.probed = 0,
			.pChip  = NULL,
			.pBank  = NULL,
			.bank_number = 0,
			.base_address = FLASH_BANK0_BASE,
			.controller_address = 0x400e0800,
			.present = 1,
			.size_bytes =  64 * 1024,
			.nsectors   =  8,
			.sector_size = 8192,
			.page_size   = 256,
		  },
//		.bank[1] = {
		  {
			.present = 0,
			.probed = 0,
			.bank_number = 1,

		  },
		},
	},

	// terminate
	{
		.chipid_cidr	= 0,
		.name			= NULL,
	}
};

/* Globals above */
/***********************************************************************
 **********************************************************************
 **********************************************************************
 **********************************************************************
 **********************************************************************
 **********************************************************************/
/* *ATMEL* style code - from the SAM3 driver code */

/**
 * Get the current status of the EEFC and
 * the value of some status bits (LOCKE, PROGE).
 * @param pPrivate - info about the bank
 * @param v        - result goes here
 */
static int
EFC_GetStatus(struct sam3_bank_private *pPrivate, uint32_t *v)
{
	int r;
	r = target_read_u32(pPrivate->pChip->target, pPrivate->controller_address + offset_EFC_FSR, v);
	LOG_DEBUG("Status: 0x%08x (lockerror: %d, cmderror: %d, ready: %d)",
			  (unsigned int)(*v),
			  ((unsigned int)((*v >> 2) & 1)),
			  ((unsigned int)((*v >> 1) & 1)),
			  ((unsigned int)((*v >> 0) & 1)));

	return r;
}

/**
 * Get the result of the last executed command.
 * @param pPrivate - info about the bank
 * @param v        - result goes here
 */
static int
EFC_GetResult(struct sam3_bank_private *pPrivate, uint32_t *v)
{
	int r;
	uint32_t rv;
	r = target_read_u32(pPrivate->pChip->target, pPrivate->controller_address + offset_EFC_FRR, &rv);
	if (v) {
		*v = rv;
	}
	LOG_DEBUG("Result: 0x%08x", ((unsigned int)(rv)));
	return r;
}

static int
EFC_StartCommand(struct sam3_bank_private *pPrivate,
				 unsigned command, unsigned argument)
{
	uint32_t n,v;
	int r;
	int retry;

	retry = 0;
 do_retry:

    // Check command & argument
    switch (command) {

	case AT91C_EFC_FCMD_WP:
	case AT91C_EFC_FCMD_WPL:
	case AT91C_EFC_FCMD_EWP:
	case AT91C_EFC_FCMD_EWPL:
		// case AT91C_EFC_FCMD_EPL:
		// case AT91C_EFC_FCMD_EPA:
	case AT91C_EFC_FCMD_SLB:
	case AT91C_EFC_FCMD_CLB:
		n = (pPrivate->size_bytes / pPrivate->page_size);
		if (argument >= n) {
			LOG_ERROR("*BUG*: Embedded flash has only %u pages", (unsigned)(n));
		}
		break;

	case AT91C_EFC_FCMD_SFB:
	case AT91C_EFC_FCMD_CFB:
		if (argument >= pPrivate->pChip->details.n_gpnvms) {
			LOG_ERROR("*BUG*: Embedded flash has only %d GPNVMs",
					  pPrivate->pChip->details.n_gpnvms);
		}
		break;

	case AT91C_EFC_FCMD_GETD:
	case AT91C_EFC_FCMD_EA:
	case AT91C_EFC_FCMD_GLB:
	case AT91C_EFC_FCMD_GFB:
	case AT91C_EFC_FCMD_STUI:
	case AT91C_EFC_FCMD_SPUI:
		if (argument != 0) {
			LOG_ERROR("Argument is meaningless for cmd: %d", command);
		}
		break;
	default:
		LOG_ERROR("Unknown command %d", command);
		break;
    }

	if (command == AT91C_EFC_FCMD_SPUI) {
		// this is a very special situation.
		// Situation (1) - error/retry - see below
		//      And we are being called recursively
		// Situation (2) - normal, finished reading unique id
	} else {
		// it should be "ready"
		EFC_GetStatus(pPrivate, &v);
		if (v & 1) {
			// then it is ready
			// we go on
		} else {
			if (retry) {
				// we have done this before
				// the controller is not responding.
				LOG_ERROR("flash controller(%d) is not ready! Error", pPrivate->bank_number);
				return ERROR_FAIL;
			} else {
				retry++;
				LOG_ERROR("Flash controller(%d) is not ready, attempting reset",
						  pPrivate->bank_number);
				// we do that by issuing the *STOP* command
				EFC_StartCommand(pPrivate, AT91C_EFC_FCMD_SPUI, 0);
				// above is recursive, and further recursion is blocked by
				// if (command == AT91C_EFC_FCMD_SPUI) above
				goto do_retry;
			}
		}
	}

	v = (0x5A << 24) | (argument << 8) | command;
	LOG_DEBUG("Command: 0x%08x", ((unsigned int)(v)));
	r = target_write_u32(pPrivate->pBank->target,
						  pPrivate->controller_address + offset_EFC_FCR,
						  v);
	if (r != ERROR_OK) {
		LOG_DEBUG("Error Write failed");
	}
	return r;
}

/**
 * Performs the given command and wait until its completion (or an error).
 * @param pPrivate - info about the bank
 * @param command  - Command to perform.
 * @param argument - Optional command argument.
 * @param status   - put command status bits here
 */
static int
EFC_PerformCommand(struct sam3_bank_private *pPrivate,
					unsigned command,
					unsigned argument,
					uint32_t *status)
{

	int r;
	uint32_t v;
	long long ms_now, ms_end;

	// default
	if (status) {
		*status = 0;
	}

	r = EFC_StartCommand(pPrivate, command, argument);
	if (r != ERROR_OK) {
		return r;
	}

	ms_end = 500 + timeval_ms();


    do {
		r = EFC_GetStatus(pPrivate, &v);
		if (r != ERROR_OK) {
			return r;
		}
		ms_now = timeval_ms();
		if (ms_now > ms_end) {
			// error
			LOG_ERROR("Command timeout");
			return ERROR_FAIL;
		}
    }
    while ((v & 1) == 0)
		;

	// error bits..
	if (status) {
		*status = (v & 0x6);
	}
	return ERROR_OK;

}





/**
 * Read the unique ID.
 * @param pPrivate - info about the bank
 * The unique ID is stored in the 'pPrivate' structure.
 */
static int
FLASHD_ReadUniqueID (struct sam3_bank_private *pPrivate)
{
	int r;
	uint32_t v;
	int x;
	// assume 0
    pPrivate->pChip->cfg.unique_id[0] = 0;
    pPrivate->pChip->cfg.unique_id[1] = 0;
    pPrivate->pChip->cfg.unique_id[2] = 0;
    pPrivate->pChip->cfg.unique_id[3] = 0;

	LOG_DEBUG("Begin");
	r = EFC_StartCommand(pPrivate, AT91C_EFC_FCMD_STUI, 0);
	if (r < 0) {
		return r;
	}

	for (x = 0 ; x < 4 ; x++) {
		r = target_read_u32(pPrivate->pChip->target,
							 pPrivate->pBank->base + (x * 4),
							 &v);
		if (r < 0) {
			return r;
		}
		pPrivate->pChip->cfg.unique_id[x] = v;
	}

    r = EFC_PerformCommand(pPrivate, AT91C_EFC_FCMD_SPUI, 0, NULL);
	LOG_DEBUG("End: R=%d, id = 0x%08x, 0x%08x, 0x%08x, 0x%08x",
			  r,
			  (unsigned int)(pPrivate->pChip->cfg.unique_id[0]),
			  (unsigned int)(pPrivate->pChip->cfg.unique_id[1]),
			  (unsigned int)(pPrivate->pChip->cfg.unique_id[2]),
			  (unsigned int)(pPrivate->pChip->cfg.unique_id[3]));
	return r;

}

/**
 * Erases the entire flash.
 * @param pPrivate - the info about the bank.
 */
static int
FLASHD_EraseEntireBank(struct sam3_bank_private *pPrivate)
{
	LOG_DEBUG("Here");
	return EFC_PerformCommand(pPrivate, AT91C_EFC_FCMD_EA, 0, NULL);
}



/**
 * Gets current GPNVM state.
 * @param pPrivate - info about the bank.
 * @param gpnvm    -  GPNVM bit index.
 * @param puthere  - result stored here.
 */
//------------------------------------------------------------------------------
static int
FLASHD_GetGPNVM(struct sam3_bank_private *pPrivate, unsigned gpnvm, unsigned *puthere)
{
	uint32_t v;
	int r;

	LOG_DEBUG("Here");
	if (pPrivate->bank_number != 0) {
		LOG_ERROR("GPNVM only works with Bank0");
		return ERROR_FAIL;
	}

	if (gpnvm >= pPrivate->pChip->details.n_gpnvms) {
		LOG_ERROR("Invalid GPNVM %d, max: %d, ignored",
				  gpnvm,pPrivate->pChip->details.n_gpnvms);
		return ERROR_FAIL;
	}

    // Get GPNVMs status
	r = EFC_PerformCommand(pPrivate, AT91C_EFC_FCMD_GFB, 0, NULL);
	if (r != ERROR_OK) {
		LOG_ERROR("Failed");
		return r;
	}

    r = EFC_GetResult(pPrivate, &v);

	if (puthere) {
		// Check if GPNVM is set
		// get the bit and make it a 0/1
		*puthere = (v >> gpnvm) & 1;
	}

	return r;
}




/**
 * Clears the selected GPNVM bit.
 * @param pPrivate info about the bank
 * @param gpnvm GPNVM index.
 * @returns 0 if successful; otherwise returns an error code.
 */
static int
FLASHD_ClrGPNVM(struct sam3_bank_private *pPrivate, unsigned gpnvm)
{
	int r;
	unsigned v;

	LOG_DEBUG("Here");
	if (pPrivate->bank_number != 0) {
		LOG_ERROR("GPNVM only works with Bank0");
		return ERROR_FAIL;
	}

	if (gpnvm >= pPrivate->pChip->details.n_gpnvms) {
		LOG_ERROR("Invalid GPNVM %d, max: %d, ignored",
				  gpnvm,pPrivate->pChip->details.n_gpnvms);
		return ERROR_FAIL;
	}

	r = FLASHD_GetGPNVM(pPrivate, gpnvm, &v);
	if (r != ERROR_OK) {
		LOG_DEBUG("Failed: %d",r);
		return r;
	}
	r = EFC_PerformCommand(pPrivate, AT91C_EFC_FCMD_CFB, gpnvm, NULL);
	LOG_DEBUG("End: %d",r);
	return r;
}



/**
 * Sets the selected GPNVM bit.
 * @param pPrivate info about the bank
 * @param gpnvm GPNVM index.
 */
static int
FLASHD_SetGPNVM(struct sam3_bank_private *pPrivate, unsigned gpnvm)
{
	int r;
	unsigned v;

	if (pPrivate->bank_number != 0) {
		LOG_ERROR("GPNVM only works with Bank0");
		return ERROR_FAIL;
	}

	if (gpnvm >= pPrivate->pChip->details.n_gpnvms) {
		LOG_ERROR("Invalid GPNVM %d, max: %d, ignored",
				  gpnvm,pPrivate->pChip->details.n_gpnvms);
		return ERROR_FAIL;
	}

	r = FLASHD_GetGPNVM(pPrivate, gpnvm, &v);
	if (r != ERROR_OK) {
		return r;
	}
	if (v) {
		// already set
		r = ERROR_OK;
	} else {
		// set it
		r = EFC_PerformCommand(pPrivate, AT91C_EFC_FCMD_SFB, gpnvm, NULL);
	}
	return r;
}


/**
 * Returns a bit field (at most 64) of locked regions within a page.
 * @param pPrivate info about the bank
 * @param v where to store locked bits
 */
static int
FLASHD_GetLockBits(struct sam3_bank_private *pPrivate, uint32_t *v)
{
	int r;
	LOG_DEBUG("Here");
    r = EFC_PerformCommand(pPrivate, AT91C_EFC_FCMD_GLB, 0, NULL);
	if (r == ERROR_OK) {
		r = EFC_GetResult(pPrivate, v);
	}
	LOG_DEBUG("End: %d",r);
	return r;
}


/**
 * Unlocks all the regions in the given address range.
 * @param pPrivate info about the bank
 * @param start_sector first sector to unlock
 * @param end_sector last (inclusive) to unlock
 */

static int
FLASHD_Unlock(struct sam3_bank_private *pPrivate,
			   unsigned start_sector,
			   unsigned end_sector)
{
	int r;
	uint32_t status;
	uint32_t pg;
	uint32_t pages_per_sector;

	pages_per_sector = pPrivate->sector_size / pPrivate->page_size;

    /* Unlock all pages */
    while (start_sector <= end_sector) {
		pg = start_sector * pages_per_sector;

        r = EFC_PerformCommand(pPrivate, AT91C_EFC_FCMD_CLB, pg, &status);
        if (r != ERROR_OK) {
            return r;
        }
        start_sector++;
    }

    return ERROR_OK;
}


/**
 * Locks regions
 * @param pPrivate - info about the bank
 * @param start_sector - first sector to lock
 * @param end_sector   - last sector (inclusive) to lock
 */
static int
FLASHD_Lock(struct sam3_bank_private *pPrivate,
			 unsigned start_sector,
			 unsigned end_sector)
{
	uint32_t status;
	uint32_t pg;
	uint32_t pages_per_sector;
	int r;

	pages_per_sector = pPrivate->sector_size / pPrivate->page_size;

    /* Lock all pages */
    while (start_sector <= end_sector) {
		pg = start_sector * pages_per_sector;

        r = EFC_PerformCommand(pPrivate, AT91C_EFC_FCMD_SLB, pg, &status);
        if (r != ERROR_OK) {
            return r;
        }
        start_sector++;