Add devmem
Change-Id: I024cbb49796f324a798596ad39c9a9691a0676a7
diff --git a/Makefile b/Makefile
index 417e298..8151fd1 100644
--- a/Makefile
+++ b/Makefile
@@ -44,7 +44,8 @@
id \
vmstat \
lsof \
- daemon
+ daemon \
+ devmem
LOCAL_SRC_FILES:= \
toolbox.c \
diff --git a/devmem.c b/devmem.c
new file mode 100644
index 0000000..0771912
--- /dev/null
+++ b/devmem.c
@@ -0,0 +1,121 @@
+#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;
+}
