ea96828d7f14c7fc1faf2dd5de42eea2da68f662
[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 unsigned long Address to write to
51 /// \param unsigned long Value to write
52 /// \param int Access type, should be 'w' (word),
53 ///        'h' (halfword), 'b'(byte)
54 /////////////////////////////////////////////////
55 int writemem(unsigned long address, unsigned long writeval, int access_type)
56 {
57     int fd;
58     void *map_base = 0;
59     void *virt_addr = 0;
60
61     off_t target = address;
62
63     // Try to open /dev/mem
64     if((fd = open("/dev/mem", O_RDWR | O_SYNC)) == -1)
65     {
66         ERROR("Opening /dev/mem/ failed!");
67         return EXIT_FAILURE;
68     };
69
70     // Map one page
71     map_base = mmap(0, MAP_SIZE, PROT_READ | PROT_WRITE, MAP_SHARED, fd, target & ~MAP_MASK);
72
73     // Check if mapping was successful
74     if(map_base == (void *) -1)
75     {
76         //Print error on failure
77         ERROR("Mapping Memory Page Failed!");
78         return EXIT_FAILURE;
79     }
80
81     //calculate virtual address
82     virt_addr = map_base + (target & MAP_MASK);
83
84     //write value to memory
85     switch(access_type)
86     {
87     case 'b':
88         *((unsigned char *) virt_addr) = writeval;
89         break;
90     case 'h':
91         *((unsigned short *) virt_addr) = writeval;
92         break;
93     case 'w':
94         *((unsigned long *) virt_addr) = writeval;
95         break;
96     default:
97         ERROR("Wrong access_type set!");
98         break;
99     }
100
101     //unmap memory
102     if(munmap(map_base, MAP_SIZE) == -1)
103     {
104         ERROR("Unmapping Memory Page Failed!");
105         return EXIT_FAILURE;
106     }
107
108     close(fd);
109     return EXIT_SUCCESS;
110 }
111
112 /////////////////////////////////////////////////
113 /// \brief This function allows reading the memory
114 ///        on the specified address.
115 ///
116 /// \param char* Address to read from
117 /// \param int Access type, should be 'w' (word),
118 ///        'h' (halfword), 'b'(byte)
119 /// \param unsigned long* pointer result variable
120 /////////////////////////////////////////////////
121 int readmem(unsigned long address, int access_type, unsigned long *result)
122 {
123     int fd;
124     void *map_base, *virt_addr;
125     unsigned long read_result = 0;
126
127     off_t target = address;
128
129     //check if result variable is a nullpointer
130     if(result == NULL)
131     {
132         //Print error on failure
133         ERROR("Result Parameter is a nullpointer!");
134         return EXIT_FAILURE;
135     }
136
137
138     // Try to open /dev/mem
139     if((fd = open("/dev/mem", O_RDWR | O_SYNC)) == -1)
140     {
141         //Print error on failure
142         ERROR("Opening /dev/mem/ failed!");
143         return EXIT_FAILURE;
144     };
145
146     // Map one page
147     map_base = mmap(0, MAP_SIZE, PROT_READ | PROT_WRITE, MAP_SHARED, fd, target & ~MAP_MASK);
148
149     // Check if mapping was successful
150     if(map_base == (void *) -1)
151     {
152         ERROR("Mapping Memory Page Failed!");
153         return EXIT_FAILURE;
154     }
155
156     //calculate virtual address
157     virt_addr = map_base + (target & MAP_MASK);
158
159     //read value
160     switch(access_type)
161     {
162     case 'b':
163         read_result = *((unsigned char *) virt_addr);
164         break;
165     case 'h':
166         read_result = *((unsigned short *) virt_addr);
167         break;
168     case 'w':
169         read_result = *((unsigned long *) virt_addr);
170         break;
171     default:
172         ERROR("Wrong access_type set!");
173         break;
174     }
175
176     // unmap memory
177     if(munmap(map_base, MAP_SIZE) == -1)
178     {
179         ERROR("Unmapping Memory Page Failed!");
180         return EXIT_FAILURE;
181     }
182
183     // return result
184     *result = read_result;
185
186     close(fd);
187     return EXIT_SUCCESS;
188 }
189