#if 0
/*
 * Copyright 2006 Freescale Semiconductor, Inc. All Rights Reserved.
 */

/*
 * The code contained herein is licensed under the GNU General Public
 * License. You may obtain a copy of the GNU General Public License
 * Version 2 or later at the following locations:
 *
 * http://www.opensource.org/licenses/gpl-license.html
 * http://www.gnu.org/copyleft/gpl.html
 */


#include <stddef.h>
#include <stdio.h>
#include <string.h>
#include <sys/types.h>
#include <stdint.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <sys/mman.h>

int g_size = 4;
unsigned long g_paddr;
int g_is_write;
uint32_t g_value = 0;
uint32_t g_count = 1;

int parse_cmdline(int argc, char ** argv)
{
	int cur_arg = 0;
	char * str;

	if (argc < 2)
		return -1;

	cur_arg++;
        if (strcmp(argv[cur_arg], "-8") == 0) {
		cur_arg++;
                g_size = 1;
        }
	else if (strcmp(argv[cur_arg], "-16") == 0) {
		cur_arg++;
                g_size = 2;
        }
	else if (strcmp(argv[cur_arg], "-32") == 0) {
		cur_arg++;
                g_size = 4;
        }
	if (cur_arg >= argc)
		return -1;

	g_paddr = strtoul(argv[cur_arg], NULL, 16);
	if (!g_paddr)
		return -1;

	if ( str = strchr(argv[cur_arg], '=') ) {
		g_is_write = 1;
		if (strlen(str) > 1) {
			str++;
			g_value = strtoul(str, NULL, 16);
			return 0;
		}
	}
	if (++cur_arg >= argc)
		return -1;

	if ((argv[cur_arg])[0] == '=' ) {
		g_is_write = 1;
		if (strlen(argv[cur_arg]) > 1) {
			(argv[cur_arg])++;
		} else {
			if (++cur_arg >= argc)
				return -1;
		}
		g_value = strtoul(argv[cur_arg], NULL, 16);
	}
	else {
		if (g_is_write)
			g_value = strtoul(argv[cur_arg], NULL, 16);
		else
			g_count = strtoul(argv[cur_arg], NULL, 16);
	}
	return 0;
}

void read_mem(void * addr, uint32_t count, uint32_t size)
{
	int i;
	uint8_t * addr8 = addr;
	uint16_t * addr16 = addr;
	uint32_t * addr32 = addr;

	switch (size)
	{
		case 1:
			for (i = 0; i < count; i++) {
				if ( (i % 16) == 0 )
					printf("\n0x%08lX: ", g_paddr);
				printf(" %02X", addr8[i]);
				g_paddr++;
			}
			break;
		case 2:
			for (i = 0; i < count; i++) {
				if ( (i % 8) == 0 )
					printf("\n0x%08lX: ", g_paddr);
				printf(" %04X", addr16[i]);
				g_paddr += 2;
			}
			break;
		case 4:
			for (i = 0; i < count; i++) {
				if ( (i % 4) == 0 )
					printf("\n0x%08lX: ", g_paddr);
				printf(" %08X", addr32[i]);
				g_paddr += 4;
			}
			break;
	}
	printf("\n\n");

}

void write_mem(void * addr, uint32_t value, uint32_t size)
{
	int i;
	uint8_t * addr8 = addr;
	uint16_t * addr16 = addr;
	uint32_t * addr32 = addr;

	switch (size)
	{
		case 1:
			*addr8 = value;
			break;
		case 2:
			*addr16 = value;
			break;
		case 4:
			*addr32 = value;
			break;
	}
}

int main(int argc, char **argv)
{
	int fd;
	void * mem;
	void * aligned_vaddr;
	unsigned long aligned_paddr;
	uint32_t aligned_size;

	if (parse_cmdline(argc, argv)) {
		printf("Usage:\n\n" \
		       "Read memory: memtool [-8 | -16 | -32] <phys addr> <count>\n" \
		       "Write memory: memtool [-8 | -16 | -32] <phys addr>=<value>\n\n" \
		       "Default access size is 32-bit.\n\nAddress, count and value are all in hex.\n");
		return 1;
	}

	/* Align address to access size */
	g_paddr &= ~(g_size - 1);

	aligned_paddr = g_paddr & ~(4096 - 1);
	aligned_size = g_paddr - aligned_paddr + (g_count * g_size);
	aligned_size = (aligned_size + 4096 - 1) & ~(4096 - 1);

	if (g_is_write)
		printf("Writing %d-bit value 0x%X to address 0x%08lX\n", g_size*8, g_value, g_paddr);
	else
		printf("Reading 0x%X count starting at address 0x%08lX\n", g_count, g_paddr);

	if ((fd = open("/dev/mem", O_RDWR, 0)) < 0)
		return 1;

	aligned_vaddr = mmap(NULL, aligned_size, PROT_READ | PROT_WRITE, MAP_SHARED, fd, aligned_paddr);
	if (aligned_vaddr == NULL) {
		printf("Error mapping address\n");
		close(fd);
		return 1;
	}

	mem = (void *)((uint32_t)aligned_vaddr + (g_paddr - aligned_paddr));

	if (g_is_write) {
		write_mem(mem, g_value, g_size);
	}
	else {
		read_mem(mem, g_count, g_size);
	}

	munmap(aligned_vaddr, aligned_size);
	close(fd);
	return 0;
}
#endif

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <sys/mman.h>
#include <stdint.h>

#include <string.h>
#include <sys/ioctl.h>
#include <sys/time.h>
#include <sys/types.h>
#include <fcntl.h>
#include <unistd.h>
#include <errno.h>

void print_help(void)
{
	printf(  "\n Usage: \n" );
	printf(  "   -a [address]           Address on which to read/write\n" );
	printf(  "   -c [count]             Number of words to read with the dump command\n" );
	printf(  "   -d                     Dump registers\n" );
	printf(  "   -r                     Perform a read action\n" );
	printf(  "   -w [data]              Perform a write action\n" );
	printf(  "   -q                     Quiet mode, only print read data\n");
}

int main(int argc, char **argv)
{
	int fd;
	int i;
	int opt;
	long page_size;
	void* virtual_address, *mem;
	int read=0, write=0,dump=1,quiet_mode=0;
	int count = 16;
	int write_value;
	off_t physical_address;
	

	while((opt = getopt(argc, argv, "h?a:dc:rw:q")) != EOF) {
		switch(opt)
		{
			case 'h':
			case '?':
				print_help();
				return EXIT_SUCCESS;
			case 'a':
				sscanf(optarg, "%x", &physical_address);
				break;
			case 'c':
				sscanf(optarg, "%x", &count);
				break;
			case 'd':
				dump = 1;
				break;

			case 'r':
				read = 1;
				dump = 0;
				break;
				
			case 'w':
				write = 1;
				dump = 0;
				sscanf(optarg, "%x", &write_value);
				break;
			case 'q':
				quiet_mode = 1;
				break;
				
			default:
				printf("Unknown option %c\n", opt);
				return EXIT_FAILURE;
		}
	}
	
	if (argc <= 1) {
		print_help();
		return EXIT_SUCCESS;
	}
	
	fd = open("/dev/mem", O_RDWR, 0);
	
	if (fd <= 0) {
		printf("Error opening /dev/mem : %s\n", strerror(errno));
		return EXIT_FAILURE;
	}

	page_size = sysconf(_SC_PAGESIZE);	
	virtual_address = mmap( 0, page_size, PROT_READ | PROT_WRITE, MAP_SHARED, fd, physical_address & ~(page_size-1) );
	if (virtual_address == NULL) {
		printf("Error mapping address\n");
		close(fd);
		return EXIT_FAILURE;
	}
	
	mem = (void *)(virtual_address + ((uint32_t)physical_address % (uint32_t) page_size));	
	
	if (write){
		*((uint32_t*) mem) = write_value;
	}

	if (read) {
		if (quiet_mode)
			printf("%08x\n", ((uint32_t*) mem)[0] );
		else 
			printf("%08x = %08x\n", physical_address, ((uint32_t*) mem)[0] );
	}

	if (dump) {
		for( i = 0; i < count; i++ ) {
			if ( ( i % 4 ) == 0 )
				printf("%08x [ ", (physical_address + (i*4) ) );
			printf("%08x ", ((uint32_t*) mem)[i] );
			if ( ( i % 4 ) == 3 )
				printf("]\n" );
		}
		printf("\n" );
	}

	munmap(virtual_address, page_size);
	
	close(fd);
	
	return 0;
}