#include <sys/mman.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <fcntl.h>
#include <ctype.h>

#define	DEVNAME		"/dev/mem"
#define BLOCKSIZE	0x00001000

static uint	dp_last_addr, dp_last_size = 0x4;
static uint	dp_last_length = 0x40;

/*
 * Print data buffer in hex and ascii form to the terminal.
 *
 * data reads are buffered so that each memory address is only read once.
 * Useful when displaying the contents of volatile registers.
 *
 * parameters:
 *    addr: Starting address to display at start of line
 *    data: pointer to data buffer
 *    width: data value width.  May be 1, 2, or 4.
 *    count: number of values to display
 *    linelen: Number of values to print per line; specify 0 for default length
 */
#define MAX_LINE_LENGTH_BYTES (64)
#define DEFAULT_LINE_LENGTH_BYTES (16)
int print_buffer (ulong addr, void* data, uint width, uint count, uint linelen)
{
	/* linebuf as a union causes proper alignment */
	union linebuf {
		u_int32_t ui[MAX_LINE_LENGTH_BYTES/sizeof(u_int32_t) + 1];
		u_int16_t us[MAX_LINE_LENGTH_BYTES/sizeof(u_int16_t) + 1];
		u_int8_t  uc[MAX_LINE_LENGTH_BYTES/sizeof(u_int8_t) + 1];
	} lb;
	int i;

	if (linelen*width > MAX_LINE_LENGTH_BYTES)
		linelen = MAX_LINE_LENGTH_BYTES / width;
	if (linelen < 1)
		linelen = DEFAULT_LINE_LENGTH_BYTES / width;

	while (count) {
		printf("%08lx:", addr);

		/* check for overflow condition */
		if (count < linelen)
			linelen = count;

		/* Copy from memory into linebuf and print hex values */
		for (i = 0; i < linelen; i++) {
			u_int32_t x;
			if (width == 4)
				x = lb.ui[i] = *(volatile u_int32_t *)data;
			else if (width == 2)
				x = lb.us[i] = *(volatile u_int16_t *)data;
			else
				x = lb.uc[i] = *(volatile u_int8_t *)data;
			printf(" %0*x", width * 2, x);
			data += width;
		}

		/* Print data in ASCII characters */
		for (i = 0; i < linelen * width; i++) {
			if (!isprint(lb.uc[i]) || lb.uc[i] >= 0x80)
				lb.uc[i] = '.';
		}
		lb.uc[i] = '\0';
		printf("    %s\n", lb.uc);

		/* update references */
		addr += linelen * width;
		count -= linelen;
	}

	return 0;
}


/* Memory Display
 *
 * Syntax:
 *	md {addr} {len} {size}
 */
#define DISP_LINE_LEN	16
int main(int argc, char *argv[])
{
	ulong	addr, length, size, tmp;
	int	rc = 0;
	int	fd, i;
	ulong	from, num, port, rdat;
	volatile u_char *iomap;

	addr = dp_last_addr;
	length = dp_last_length;
	size = dp_last_size;

	if( argc > 1 ) addr = (ulong) strtoul( argv[1], NULL, 16 );
	if( argc > 2 ) length = (ulong) strtoul( argv[2], NULL, 16 );
	if( argc > 3 ) size = (ulong) strtoul( argv[3], NULL, 16 );
	if( size != 1 && size != 2 && size != 4 ) size = 4;

	/* Open Physical Memory Device */
	fd = open( DEVNAME, O_RDWR | O_SYNC );
	if( fd <= 0 ){
		perror( DEVNAME );
		exit(1);
	}

	/* Do mmap with fd */
	tmp = ( BLOCKSIZE - 1 ) & addr;
	from = addr - tmp;
	num = BLOCKSIZE;
	iomap = mmap( 0, num, PROT_READ | PROT_WRITE, MAP_SHARED, fd, from );
	if( iomap < 0 ){
		perror( DEVNAME );
		exit(1);
	}
#if 0
	printf("IOmem mmaped at %p\n", iomap);

	port = addr;

	for( i = 0 ; i < length ; i++ ){
		rdat = *( volatile unsigned long * )(iomap + port - from );
		printf("0x%08x = 0x%08x\n", port, rdat);
		port += 4;
	}
#else
	print_buffer(addr, (void*)(iomap + tmp), size, length, DISP_LINE_LEN/size);
	addr += size*length;
#endif
	/* Close mmap */
	munmap( (void *)iomap, num );

	/* Close device */
	close( fd );

	dp_last_addr = addr;
	dp_last_length = length;
	dp_last_size = size;

	return (rc);
}

