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

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

/* Memory Write
 *
 * Syntax:
 *	mw {addr} {writeval} {size}
 */
int main(int argc, char *argv[])
{
	ulong	addr, writeval, size, tmp;
	int	rc = 0;
	int	fd, i;
	ulong	from, num, port, rdat;
	volatile u_char *iomap;

	if( argc < 3 ){
		printf("Usage: mw {addr} {data} {size}\n");
		return rc;
	}

	if( argc > 1 ) addr = (ulong) strtoul( argv[1], NULL, 16 );
	if( argc > 2 ) writeval = (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( size == 4 )
		*((ulong *)(iomap + tmp)) = (ulong)writeval;
	else if( size == 2 )
		*((ushort *)(iomap + tmp)) = (ushort)writeval;
	else
		*((u_char *)(iomap + tmp)) = (u_char)writeval;
	addr += size;
#if 0
	printf("IOmem mmaped at %p\n", iomap);
#endif
	/* Close mmap */
	munmap( (void *)iomap, num );

	/* Close device */
	close( fd );

	return (rc);
}

