Fixed missing vitual address calulation in writemem, now devmem2 module works as...
[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: assumes a 4k page size and that sizeof(unsigned long) == 4
7 // Revision: 1.2
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 #include "devmem2.h"
25
26 #define MAP_SIZE 4096UL
27 #define MAP_MASK (MAP_SIZE - 1)
28
29
30 /////////////////////////////////////////////////
31 /// \brief This function is called by the module
32 ///        if an error occurs.
33 ///
34 /// \param char* Custom error Message.
35 /////////////////////////////////////////////////
36 static void ERROR(char* custom)
37 {
38     //print custom error message
39     fprintf(stderr, "Error: %s \n", custom);
40
41     //print further information
42     fprintf(stderr, "Info: %d, %s\n",
43             errno, strerror(errno));
44 }
45
46 /////////////////////////////////////////////////
47 /// \brief This function allows writing to memory
48 ///        on the specified address.
49 ///
50 /// \param char* Address to write to
51 /// \param char* Value to write
52 /// \param int Access type, should be 'w' (word),
53 ///        'h' (halfword), 'b'(byte)
54 /////////////////////////////////////////////////
55 int writemem(char* address, char* value, int access_type)
56 {
57     int fd;
58     void *map_base = 0;
59     void *virt_addr = 0;
60     unsigned long writeval = 0;
61
62     off_t target = strtoul(address, 0, 0);
63
64
65     if((fd = open("/dev/mem", O_RDWR | O_SYNC)) == -1)
66     {
67         ERROR("Opening /dev/mem/ failed!");
68         return EXIT_FAILURE;
69     };
70
71     // Map one page
72     map_base = mmap(0, MAP_SIZE, PROT_READ | PROT_WRITE, MAP_SHARED, fd, target & ~MAP_MASK);
73
74     if(map_base == (void *) -1)
75     {
76         ERROR("Mapping Memory Page Failed!");
77         return EXIT_FAILURE;
78     }
79
80
81     virt_addr = map_base + (target & MAP_MASK);
82     writeval = strtoul(value, 0, 0);
83     switch(access_type)
84     {
85     case 'b':
86         *((unsigned char *) virt_addr) = writeval;
87         break;
88     case 'h':
89         *((unsigned short *) virt_addr) = writeval;
90         break;
91     case 'w':
92         *((unsigned long *) virt_addr) = writeval;
93         break;
94     default:
95         ERROR("Wrong access_type set!");
96         break;
97     }
98
99
100     if(munmap(map_base, MAP_SIZE) == -1)
101     {
102         ERROR("Unmapping Memory Page Failed!");
103         return EXIT_FAILURE;
104     }
105
106     close(fd);
107     return EXIT_SUCCESS;
108 }
109
110 /////////////////////////////////////////////////
111 /// \brief This function allows reading the memory
112 ///        on the specified address.
113 ///
114 /// \param char* Address to read from
115 /// \param int Access type, should be 'w' (word),
116 ///        'h' (halfword), 'b'(byte)
117 /// \param unsigned long* pointer result variable
118 /////////////////////////////////////////////////
119 int readmem(char* address, int access_type, unsigned long *result)
120 {
121     int fd;
122     void *map_base, *virt_addr;
123     off_t target;
124     unsigned long read_result = 0;
125
126     target = strtoul(address, 0, 0);
127
128     if(result == NULL)
129     {
130         //Print error on failure
131         ERROR("Result Parameter is a nullpointer!");
132         return EXIT_FAILURE;
133     }
134
135
136     // Try to open /dev/mem
137     if((fd = open("/dev/mem", O_RDWR | O_SYNC)) == -1)
138     {
139         //Print error on failure
140         ERROR("Opening /dev/mem/ failed!");
141         return EXIT_FAILURE;
142     };
143
144     // Map one page
145     map_base = mmap(0, MAP_SIZE, PROT_READ | PROT_WRITE, MAP_SHARED, fd, target & ~MAP_MASK);
146
147     // Check if mapping was successful
148     if(map_base == (void *) -1)
149     {
150         ERROR("Mapping Memory Page Failed!");
151         return EXIT_FAILURE;
152     }
153
154     virt_addr = map_base + (target & MAP_MASK);
155     switch(access_type)
156     {
157     case 'b':
158         read_result = *((unsigned char *) virt_addr);
159         break;
160     case 'h':
161         read_result = *((unsigned short *) virt_addr);
162         break;
163     case 'w':
164         read_result = *((unsigned long *) virt_addr);
165         break;
166     default:
167         ERROR("Wrong access_type set!");
168         break;
169     }
170
171     if(munmap(map_base, MAP_SIZE) == -1)
172     {
173         ERROR("Unmapping Memory Page Failed!");
174         return EXIT_FAILURE;
175     }
176
177     *result = read_result;
178
179     close(fd);
180     return EXIT_SUCCESS;
181 }
182