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