1e8ec8caf71222abecb02635d62fa4fbe91a4cf5
[allwinner_tvout_manipulator.git] / src / devmem2.c
1 ///////////////////////////////////////////////////////////////////////////
2 // Workfile: devmem2.c (Implementation)
3 // Author: Jan-Derk Bakker, Daniel Giritzer
4 // Date: 2017-06-02
5 // Description: Simple module to read/write from/to any location in memory.
6 // Remarks: -
7 // Revision: 1
8 // Copyright (C) 2000, Jan-Derk Bakker (J.D.Bakker@its.tudelft.nl)
9 // Copyright (C) 2017, Daniel Giritzer (giri@nwrk.biz)
10 ///////////////////////////////////////////////////////////////////////////
11 #include <stdio.h>
12 #include <stdlib.h>
13 #include <unistd.h>
14 #include <string.h>
15 #include <string.h>
16 #include <errno.h>
17 #include <signal.h>
18 #include <fcntl.h>
19 #include <ctype.h>
20 #include <termios.h>
21 #include <sys/types.h>
22 #include <sys/mman.h>
23
24 #define MAP_SIZE 4096UL
25 #define MAP_MASK (MAP_SIZE - 1)
26
27 static void ERROR(char* custom)
28 {
29     fprintf(stderr, "Error: %s \n", custom);
30     fprintf(stderr, "Error Nr: %d, %s\n",
31             errno, strerror(errno));
32 }
33
34 int writemem(char* address, char* value, int access_type)
35 {
36     int fd;
37     void *map_base = 0;
38     void *virt_addr = 0;
39     unsigned long writeval = 0;
40
41     off_t target = strtoul(address, 0, 0);
42
43
44     if((fd = open("/dev/mem", O_RDWR | O_SYNC)) == -1)
45     {
46         ERROR("Opening /dev/mem/ failed!");
47         return EXIT_FAILURE;
48     };
49
50     // Map one page
51     map_base = mmap(0, MAP_SIZE, PROT_READ | PROT_WRITE, MAP_SHARED, fd, target & ~MAP_MASK);
52
53     if(map_base == (void *) -1)
54     {
55         ERROR("Mapping Memory Page Failed!");
56         return EXIT_FAILURE;
57     }
58
59
60     writeval = strtoul(value, 0, 0);
61     switch(access_type)
62     {
63     case 'b':
64         *((unsigned char *) virt_addr) = writeval;
65         break;
66     case 'h':
67         *((unsigned short *) virt_addr) = writeval;
68         break;
69     case 'w':
70         *((unsigned long *) virt_addr) = writeval;
71         break;
72     default:
73         ERROR("Wrong access_type set!");
74         break;
75     }
76
77
78     if(munmap(map_base, MAP_SIZE) == -1)
79     {
80         ERROR("Unmapping Memory Page Failed!");
81         return EXIT_FAILURE;
82     }
83
84     close(fd);
85     return EXIT_SUCCESS;
86 }
87
88 int readmem(char* address, int access_type, unsigned long read_result)
89 {
90     int fd;
91     void *map_base, *virt_addr;
92     off_t target;
93
94     target = strtoul(address, 0, 0);
95
96
97     // Try to open /dev/mem
98     if((fd = open("/dev/mem", O_RDWR | O_SYNC)) == -1)
99     {
100         //Print error on failure
101         ERROR("Opening /dev/mem/ failed!");
102         return EXIT_FAILURE;
103     };
104
105     // Map one page
106     map_base = mmap(0, MAP_SIZE, PROT_READ | PROT_WRITE, MAP_SHARED, fd, target & ~MAP_MASK);
107
108     // Check if mapping was successful
109     if(map_base == (void *) -1)
110     {
111         ERROR("Mapping Memory Page Failed!");
112         return EXIT_FAILURE;
113     }
114
115     virt_addr = map_base + (target & MAP_MASK);
116     switch(access_type)
117     {
118     case 'b':
119         read_result = *((unsigned char *) virt_addr);
120         break;
121     case 'h':
122         read_result = *((unsigned short *) virt_addr);
123         break;
124     case 'w':
125         read_result = *((unsigned long *) virt_addr);
126         break;
127     default:
128         ERROR("Wrong access_type set!");
129         break;
130     }
131
132     if(munmap(map_base, MAP_SIZE) == -1)
133     {
134         ERROR("Unmapping Memory Page Failed!");
135         return EXIT_FAILURE;
136     }
137
138     close(fd);
139     return EXIT_SUCCESS;
140 }
141