| --- devmem2.c 2004-08-05 01:55:25.000000000 +0200 |
| +++ devmem2_modif.c 2011-01-13 15:48:37.798799784 +0100 |
| @@ -45,12 +45,16 @@ |
| #define MAP_SIZE 4096UL |
| #define MAP_MASK (MAP_SIZE - 1) |
| |
| +static inline void *fixup_addr(void *addr, size_t size); |
| + |
| int main(int argc, char **argv) { |
| int fd; |
| void *map_base, *virt_addr; |
| - unsigned long read_result, writeval; |
| + unsigned long read_result, write_val; |
| off_t target; |
| int access_type = 'w'; |
| + char fmt_str[128]; |
| + size_t data_size; |
| |
| if(argc < 2) { |
| fprintf(stderr, "\nUsage:\t%s { address } [ type [ data ] ]\n" |
| @@ -79,38 +83,51 @@ |
| virt_addr = map_base + (target & MAP_MASK); |
| switch(access_type) { |
| case 'b': |
| + data_size = sizeof(unsigned char); |
| + virt_addr = fixup_addr(virt_addr, data_size); |
| read_result = *((unsigned char *) virt_addr); |
| break; |
| case 'h': |
| + data_size = sizeof(unsigned short); |
| + virt_addr = fixup_addr(virt_addr, data_size); |
| read_result = *((unsigned short *) virt_addr); |
| break; |
| case 'w': |
| + data_size = sizeof(unsigned long); |
| + virt_addr = fixup_addr(virt_addr, data_size); |
| read_result = *((unsigned long *) virt_addr); |
| break; |
| default: |
| fprintf(stderr, "Illegal data type '%c'.\n", access_type); |
| exit(2); |
| } |
| - printf("Value at address 0x%X (%p): 0x%X\n", target, virt_addr, read_result); |
| + sprintf(fmt_str, "Read at address 0x%%08lX (%%p): 0x%%0%dlX\n", 2*data_size); |
| + printf(fmt_str, (unsigned long)target, virt_addr, read_result); |
| fflush(stdout); |
| |
| if(argc > 3) { |
| - writeval = strtoul(argv[3], 0, 0); |
| + write_val = strtoul(argv[3], 0, 0); |
| switch(access_type) { |
| case 'b': |
| - *((unsigned char *) virt_addr) = writeval; |
| + virt_addr = fixup_addr(virt_addr, sizeof(unsigned char)); |
| + *((unsigned char *) virt_addr) = write_val; |
| read_result = *((unsigned char *) virt_addr); |
| break; |
| case 'h': |
| - *((unsigned short *) virt_addr) = writeval; |
| + virt_addr = fixup_addr(virt_addr, sizeof(unsigned short)); |
| + *((unsigned short *) virt_addr) = write_val; |
| read_result = *((unsigned short *) virt_addr); |
| break; |
| case 'w': |
| - *((unsigned long *) virt_addr) = writeval; |
| + virt_addr = fixup_addr(virt_addr, sizeof(unsigned long)); |
| + *((unsigned long *) virt_addr) = write_val; |
| read_result = *((unsigned long *) virt_addr); |
| break; |
| } |
| - printf("Written 0x%X; readback 0x%X\n", writeval, read_result); |
| + sprintf(fmt_str, "Write at address 0x%%08lX (%%p): 0x%%0%dlX, " |
| + "readback 0x%%0%dlX\n", 2*data_size, 2*data_size); |
| + printf(fmt_str, (unsigned long)target, virt_addr, |
| + write_val, read_result); |
| fflush(stdout); |
| } |
| |
| @@ -119,3 +136,12 @@ |
| return 0; |
| } |
| |
| +static inline void *fixup_addr(void *addr, size_t size) |
| +{ |
| +#ifdef FORCE_STRICT_ALIGNMENT |
| + unsigned long aligned_addr = (unsigned long)addr; |
| + aligned_addr &= ~(size - 1); |
| + addr = (void *)aligned_addr; |
| +#endif |
| + return addr; |
| +} |