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

typedef unsigned int uint32;
typedef unsigned short uint16;
typedef unsigned char uint8;

int devmem_main(int argc, char *argv[]) {

  if (argc < 2 || argc > 4) {
    fprintf(stderr, "usage: devmem <addr> [default:32|16|8] [data]\n");
    return 1;
  }

  off_t addr;
  uint32 data = 0;
  int wordsize = 32;

  addr = strtoul(argv[1], NULL, 0);
  if (errno) {
    perror("Invalid address specified");
    return 1;
  }

  if (argc > 2) {
    wordsize = strtol(argv[2], NULL, 0);

    if (errno || (wordsize != 32 && wordsize != 16 && wordsize != 8)) {
      fprintf(stderr, "Invalid wordsize specified\n");
      return 1;
    }
  }

  if (argc == 4) {
    data = strtoul(argv[3], NULL, 0);
    if (errno) {
      perror("Invalid data specified");
      return 1;
    }
  }


  int mem_fd;
  void *base, *offset;
  long pagesize;

  pagesize = sysconf(_SC_PAGE_SIZE);

  if (pagesize <= 0) {
    return 1;
  }

  mem_fd = open("/dev/mem", O_SYNC | O_RDWR);
  if (mem_fd < 0) {
    perror("open /dev/mem failed");
    return 1;
  }

  base = mmap(NULL, 2*pagesize, PROT_READ | PROT_WRITE,
              MAP_SHARED | MAP_FIXED, mem_fd, addr & ~(pagesize-1));

  if (base == MAP_FAILED) {
    perror("mmap failed");
    return 1;
  }

  offset = base + (addr & (pagesize-1));

  switch(wordsize) {
    case 32:
      fprintf(stdout, "0x%08x\n", *(uint32*)offset);
      break;

    case 16:
      fprintf(stdout, "0x%04x\n", *(uint16*)offset);
      break;

    case 8:
      fprintf(stdout, "0x%02x\n", *(uint8*)offset);
      break;
  }

  if (argc == 4) {
    switch(wordsize) {
      case 32:
        *(uint32*)offset = (uint32)data;
        fprintf(stdout, "0x%08x\n", *(uint32*)offset);
        break;

      case 16:
        *(uint16*)offset = (uint16)data;
        fprintf(stdout, "0x%04x\n", *(uint16*)offset);
        break;

      case 8:
        *(uint8*)offset = (uint8)data;
        fprintf(stdout, "0x%02x\n", *(uint8*)offset);
        break;
    }
  }


  if (munmap(base, 2*pagesize)) {
    perror("munmap failed");
    return 1;
  }

  if (close(mem_fd)) {
    perror("cannot close /dev/mem");
    return 1;
  }

  return 0;
}