abcd7e3a3727c7672d1e1f5ef4fbf7f2764d2027
[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 #include "devmem2.h"
25
26 #define MAP_SIZE 4096UL
27 #define MAP_MASK (MAP_SIZE - 1)
28
29 static void ERROR(char* custom)
30 {
31     fprintf(stderr, "Error: %s \n", custom);
32     fprintf(stderr, "Info: %d, %s\n",
33             errno, strerror(errno));
34 }
35
36 int writemem(char* address, char* value, int access_type)
37 {
38     int fd;
39     void *map_base = 0;
40     void *virt_addr = 0;
41     unsigned long writeval = 0;
42
43     off_t target = strtoul(address, 0, 0);
44
45
46     if((fd = open("/dev/mem", O_RDWR | O_SYNC)) == -1)
47     {
48         ERROR("Opening /dev/mem/ failed!");
49         return EXIT_FAILURE;
50     };
51
52     // Map one page
53     map_base = mmap(0, MAP_SIZE, PROT_READ | PROT_WRITE, MAP_SHARED, fd, target & ~MAP_MASK);
54
55     if(map_base == (void *) -1)
56     {
57         ERROR("Mapping Memory Page Failed!");
58         return EXIT_FAILURE;
59     }
60
61
62     writeval = strtoul(value, 0, 0);
63     switch(access_type)
64     {
65     case 'b':
66         *((unsigned char *) virt_addr) = writeval;
67         break;
68     case 'h':
69         *((unsigned short *) virt_addr) = writeval;
70         break;
71     case 'w':
72         *((unsigned long *) virt_addr) = writeval;
73         break;
74     default:
75         ERROR("Wrong access_type set!");
76         break;
77     }
78
79
80     if(munmap(map_base, MAP_SIZE) == -1)
81     {
82         ERROR("Unmapping Memory Page Failed!");
83         return EXIT_FAILURE;
84     }
85
86     close(fd);
87     return EXIT_SUCCESS;
88 }
89
90 int readmem(char* address, int access_type, unsigned long *result)
91 {
92     int fd;
93     void *map_base, *virt_addr;
94     off_t target;
95     unsigned long read_result = 0;
96
97     target = strtoul(address, 0, 0);
98
99     if(result == NULL)
100     {
101         //Print error on failure
102         ERROR("Result Parameter is a nullpointer!");
103         return EXIT_FAILURE;
104     }
105
106
107     // Try to open /dev/mem
108     if((fd = open("/dev/mem", O_RDWR | O_SYNC)) == -1)
109     {
110         //Print error on failure
111         ERROR("Opening /dev/mem/ failed!");
112         return EXIT_FAILURE;
113     };
114
115     // Map one page
116     map_base = mmap(0, MAP_SIZE, PROT_READ | PROT_WRITE, MAP_SHARED, fd, target & ~MAP_MASK);
117
118     // Check if mapping was successful
119     if(map_base == (void *) -1)
120     {
121         ERROR("Mapping Memory Page Failed!");
122         return EXIT_FAILURE;
123     }
124
125     virt_addr = map_base + (target & MAP_MASK);
126     switch(access_type)
127     {
128     case 'b':
129         read_result = *((unsigned char *) virt_addr);
130         break;
131     case 'h':
132         read_result = *((unsigned short *) virt_addr);
133         break;
134     case 'w':
135         read_result = *((unsigned long *) virt_addr);
136         break;
137     default:
138         ERROR("Wrong access_type set!");
139         break;
140     }
141
142     if(munmap(map_base, MAP_SIZE) == -1)
143     {
144         ERROR("Unmapping Memory Page Failed!");
145         return EXIT_FAILURE;
146     }
147
148     *result = read_result;
149
150     close(fd);
151     return EXIT_SUCCESS;
152 }
153