| Andrew Geissler | d583833 | 2022-05-27 11:33:10 -0500 | [diff] [blame^] | 1 | --- devmem2.c 2004-08-05 01:55:25.000000000 +0200 |
| 2 | +++ devmem2_modif.c 2011-01-13 15:48:37.798799784 +0100 |
| 3 | @@ -45,12 +45,16 @@ |
| 4 | #define MAP_SIZE 4096UL |
| 5 | #define MAP_MASK (MAP_SIZE - 1) |
| 6 | |
| 7 | +static inline void *fixup_addr(void *addr, size_t size); |
| 8 | + |
| 9 | int main(int argc, char **argv) { |
| 10 | int fd; |
| 11 | void *map_base, *virt_addr; |
| 12 | - unsigned long read_result, writeval; |
| 13 | + unsigned long read_result, write_val; |
| 14 | off_t target; |
| 15 | int access_type = 'w'; |
| 16 | + char fmt_str[128]; |
| 17 | + size_t data_size; |
| 18 | |
| 19 | if(argc < 2) { |
| 20 | fprintf(stderr, "\nUsage:\t%s { address } [ type [ data ] ]\n" |
| 21 | @@ -79,38 +83,51 @@ |
| 22 | virt_addr = map_base + (target & MAP_MASK); |
| 23 | switch(access_type) { |
| 24 | case 'b': |
| 25 | + data_size = sizeof(unsigned char); |
| 26 | + virt_addr = fixup_addr(virt_addr, data_size); |
| 27 | read_result = *((unsigned char *) virt_addr); |
| 28 | break; |
| 29 | case 'h': |
| 30 | + data_size = sizeof(unsigned short); |
| 31 | + virt_addr = fixup_addr(virt_addr, data_size); |
| 32 | read_result = *((unsigned short *) virt_addr); |
| 33 | break; |
| 34 | case 'w': |
| 35 | + data_size = sizeof(unsigned long); |
| 36 | + virt_addr = fixup_addr(virt_addr, data_size); |
| 37 | read_result = *((unsigned long *) virt_addr); |
| 38 | break; |
| 39 | default: |
| 40 | fprintf(stderr, "Illegal data type '%c'.\n", access_type); |
| 41 | exit(2); |
| 42 | } |
| 43 | - printf("Value at address 0x%X (%p): 0x%X\n", target, virt_addr, read_result); |
| 44 | + sprintf(fmt_str, "Read at address 0x%%08lX (%%p): 0x%%0%dlX\n", 2*data_size); |
| 45 | + printf(fmt_str, (unsigned long)target, virt_addr, read_result); |
| 46 | fflush(stdout); |
| 47 | |
| 48 | if(argc > 3) { |
| 49 | - writeval = strtoul(argv[3], 0, 0); |
| 50 | + write_val = strtoul(argv[3], 0, 0); |
| 51 | switch(access_type) { |
| 52 | case 'b': |
| 53 | - *((unsigned char *) virt_addr) = writeval; |
| 54 | + virt_addr = fixup_addr(virt_addr, sizeof(unsigned char)); |
| 55 | + *((unsigned char *) virt_addr) = write_val; |
| 56 | read_result = *((unsigned char *) virt_addr); |
| 57 | break; |
| 58 | case 'h': |
| 59 | - *((unsigned short *) virt_addr) = writeval; |
| 60 | + virt_addr = fixup_addr(virt_addr, sizeof(unsigned short)); |
| 61 | + *((unsigned short *) virt_addr) = write_val; |
| 62 | read_result = *((unsigned short *) virt_addr); |
| 63 | break; |
| 64 | case 'w': |
| 65 | - *((unsigned long *) virt_addr) = writeval; |
| 66 | + virt_addr = fixup_addr(virt_addr, sizeof(unsigned long)); |
| 67 | + *((unsigned long *) virt_addr) = write_val; |
| 68 | read_result = *((unsigned long *) virt_addr); |
| 69 | break; |
| 70 | } |
| 71 | - printf("Written 0x%X; readback 0x%X\n", writeval, read_result); |
| 72 | + sprintf(fmt_str, "Write at address 0x%%08lX (%%p): 0x%%0%dlX, " |
| 73 | + "readback 0x%%0%dlX\n", 2*data_size, 2*data_size); |
| 74 | + printf(fmt_str, (unsigned long)target, virt_addr, |
| 75 | + write_val, read_result); |
| 76 | fflush(stdout); |
| 77 | } |
| 78 | |
| 79 | @@ -119,3 +136,12 @@ |
| 80 | return 0; |
| 81 | } |
| 82 | |
| 83 | +static inline void *fixup_addr(void *addr, size_t size) |
| 84 | +{ |
| 85 | +#ifdef FORCE_STRICT_ALIGNMENT |
| 86 | + unsigned long aligned_addr = (unsigned long)addr; |
| 87 | + aligned_addr &= ~(size - 1); |
| 88 | + addr = (void *)aligned_addr; |
| 89 | +#endif |
| 90 | + return addr; |
| 91 | +} |