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

extern int errno;

static void syntax(void)
{
	fprintf(stderr, "simple raw memory management utility\n");
	fprintf(stderr, "usage:\n");
	fprintf(stderr, "- read:  mem [-1|-2|-4] 0xADDR\n");
	fprintf(stderr, "- write: mem [-1|-2|-4] 0xADDR 0xVALUE\n");
	fprintf(stderr, "-1, -2, -4: access size in bytes\n");
	exit(EXIT_FAILURE);
}

struct map_t {
	union {
		void *p;
		unsigned long *sz4;
		unsigned short *sz2;
		unsigned char *sz1;
	} volatile p;

	size_t size;

	int fd;
};

static void map_memory(unsigned long addr, size_t size, char mode, struct map_t *map)
{
	void *p;

	map->fd = open("/dev/mem", (mode == 'r') ? O_RDONLY : O_WRONLY);
	if (map->fd == -1) {
		fprintf(stderr, "can't open /dev/mem: %s\n", strerror(errno));
		exit(EXIT_FAILURE);
	}

	p = mmap(NULL, size, (mode == 'r') ? PROT_READ : PROT_WRITE, MAP_SHARED, map->fd, addr);
	if (p == MAP_FAILED) {
		fprintf(stderr, "can't map memory: %s\n", strerror(errno));
		exit(EXIT_FAILURE);
	}

	map->p.p = p;
	map->size = size;
}

static void unmap_memory(const struct map_t *map)
{
	munmap(map->p.p, map->size);
	close(map->fd);
}

static unsigned long do_read(int size, unsigned long addr)
{
	unsigned long rs;
	struct map_t map;

	map_memory(addr, size, 'r', &map);

	if (size == 1)
		rs = *map.p.sz1;
	else if (size == 2)
		rs = *map.p.sz2;
	else
		rs = *map.p.sz4;

	unmap_memory(&map);

	return rs;
}

static void do_write(int size, unsigned long addr, unsigned long data)
{
	struct map_t map;

	map_memory(addr, size, 'w', &map);

	if (size == 1)
		*map.p.sz1 = data;
	else if (size == 2)
		*map.p.sz2 = data;
	else
		*map.p.sz4 = data;

	unmap_memory(&map);
}

int main(int ac, char * const av[])
{
	unsigned long addr, data;
	int size = 1;
	char fs[32];

	ac--;
	av++;

	if (ac < 1)
		syntax();

	if (av[0][0] == '-') {
		if (av[0][1] == '1' || av[0][1] == '2' || av[0][1] == '4') {
			size = av[0][1] - '0';
			ac--;
			av++;
		} else
			syntax();
	}

	if (ac < 1)
		syntax();

	addr = strtoll(av[0], NULL, 0);

	if (size == 1)
		strncpy(fs, "[%c] 0x%08X = 0x%02X\n", sizeof(fs));
	else if (size == 2)
		strncpy(fs, "[%c] 0x%08X = 0x%04X\n", sizeof(fs));
	else
		strncpy(fs, "[%c] 0x%08X = 0x%08X\n", sizeof(fs));

	if (ac == 2) {
		data = strtoll(av[1], NULL, 0);
		printf(fs, 'w', addr, data);
		do_write(size, addr, data);
	}

	data = do_read(size, addr);
	printf(fs, 'r', addr, data);

	return 0;
}