1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
|
/*
* pwm.c: Simple program control hardware PWM on Omega2
*
* Copyright (C) 2017, Alexey 'Cluster' Avdyukhin (clusterrr@clusterrr.com)
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <errno.h>
#include <fcntl.h>
#include <ctype.h>
#include <sys/types.h>
#include <sys/mman.h>
#include <inttypes.h>
#define printerr(fmt,...) do { fprintf(stderr, fmt, ## __VA_ARGS__); fflush(stderr); } while(0)
#define PWM_ENABLE 0x10005000 // PWM Enable register
#define PWM0_CON 0x10005010 // PWM0 Control register
#define PWM0_HDURATION 0x10005014 // PWM0 High Duration register
#define PWM0_LDURATION 0x10005018 // PWM0 Low Duration register
#define PWM0_GDURATION 0x1000501C // PWM0 Guard Duration register
#define PWM0_SEND_DATA0 0x10005030 // PWM0 Send Data0 register
#define PWM0_SEND_DATA1 0x10005034 // PWM0 Send Data1 register
#define PWM0_WAVE_NUM 0x10005038 // PWM0 Wave Number register
#define PWM0_DATA_WIDTH 0x1000503C // PWM0 Data Width register
#define PWM0_THRESH 0x10005040 // PWM0 Thresh register
#define PWM0_SEND_WAVENUM 0x10005044 // PWM0 Send Wave Number register
#define CLKSEL_100KHZ 0b00000000
#define CLKSEL_40MHZ 0b00001000
#define CLKDIV_1 0b00000000
#define CLKDIV_2 0b00000001
#define CLKDIV_4 0b00000010
#define CLKDIV_8 0b00000011
#define CLKDIV_16 0b00000100
#define CLKDIV_32 0b00000101
#define CLKDIV_64 0b00000110
#define CLKDIV_128 0b00000111
uint32_t devmem(uint32_t target, uint8_t size, uint8_t write, uint32_t value)
{
int fd;
void *map_base, *virt_addr;
uint32_t pagesize = (unsigned)getpagesize(); /* or sysconf(_SC_PAGESIZE) */
uint32_t map_size = pagesize;
uint32_t offset;
uint32_t result;
offset = (unsigned int)(target & (pagesize-1));
if (offset + size > pagesize ) {
// Access straddles page boundary: add another page:
map_size += pagesize;
}
fd = open("/dev/mem", O_RDWR | O_SYNC);
if (fd == -1) {
printerr("Error opening /dev/mem (%d) : %s\n", errno, strerror(errno));
exit(1);
}
map_base = mmap(0, map_size, PROT_READ | PROT_WRITE, MAP_SHARED,
fd,
target & ~((typeof(target))pagesize-1));
if (map_base == (void *) -1) {
printerr("Error mapping (%d) : %s\n", errno, strerror(errno));
exit(1);
}
virt_addr = map_base + offset;
if (write)
{
switch (size) {
case 1:
*((volatile uint8_t *) virt_addr) = value;
break;
case 2:
*((volatile uint16_t *) virt_addr) = value;
break;
case 4:
*((volatile uint32_t *) virt_addr) = value;
break;
}
}
switch (size) {
case 1:
result = *((volatile uint8_t *) virt_addr);
break;
case 2:
result = *((volatile uint16_t *) virt_addr);
break;
case 4:
result = *((volatile uint32_t *) virt_addr);
break;
}
if (munmap(map_base, map_size) != 0) {
printerr("ERROR munmap (%d) %s\n", errno, strerror(errno));
exit(1);
}
close(fd);
return result;
}
static void usage(const char *cmd)
{
fprintf(stderr,
"\nUsage:\t%s <channel> <frequency> [duty]\n",
cmd);
}
static uint32_t get_base_freq(uint16_t mode)
{
int i;
uint32_t r;
if (!(mode & CLKSEL_40MHZ))
r = 100000;
else
r = 40000000;
for (i = 0; i < (mode & 0b00000111); i++) r /= 2;
return r;
}
static void pwm_raw(uint8_t channel, uint16_t mode, uint16_t duration_h, uint16_t duration_l)
{
uint32_t enable = devmem(PWM_ENABLE, 4, 0, 0);
enable &= ~((uint32_t)(1<<channel));
devmem(PWM_ENABLE, 4, 1, enable);
// Need to disable PWM?
if (!duration_h || !duration_l)
return;
uint32_t reg_offset = 0x40 * channel;
devmem(PWM0_CON + reg_offset, 4, 1, 0x7000 | mode);
devmem(PWM0_HDURATION + reg_offset, 4, 1, duration_h - 1);
devmem(PWM0_LDURATION + reg_offset, 4, 1, duration_l - 1);
devmem(PWM0_GDURATION + reg_offset, 4, 1, (duration_h + duration_l) / 2 - 1);
devmem(PWM0_SEND_DATA0 + reg_offset, 4, 1, 0x55555555);
devmem(PWM0_SEND_DATA1 + reg_offset, 4, 1, 0x55555555);
devmem(PWM0_WAVE_NUM + reg_offset, 4, 1, 0);
enable |= 1<<channel;
devmem(PWM_ENABLE, 4, 1, enable);
}
static int pwm(uint8_t channel, uint32_t freq, uint8_t duty)
{
// Need to disable PWM?
if (!freq)
{
pwm_raw(channel, 0, 0, 0);
return 0;
}
uint32_t reg_offset = 0x40 * channel;
uint16_t mode = 0;
uint32_t base_freq = 0;
if (get_base_freq(CLKSEL_40MHZ | CLKDIV_1) / freq < 0xFFFF)
mode = CLKSEL_40MHZ | CLKDIV_1;
else if (get_base_freq(CLKSEL_40MHZ | CLKDIV_2) / freq < 0xFFFF)
mode = CLKSEL_40MHZ | CLKDIV_2;
else if (get_base_freq(CLKSEL_40MHZ | CLKDIV_4) / freq < 0xFFFF)
mode = CLKSEL_40MHZ | CLKDIV_4;
else if (get_base_freq(CLKSEL_40MHZ | CLKDIV_8) / freq < 0xFFFF)
mode = CLKSEL_40MHZ | CLKDIV_8;
else if (get_base_freq(CLKSEL_40MHZ | CLKDIV_16) / freq < 0xFFFF)
mode = CLKSEL_40MHZ | CLKDIV_16;
else if (get_base_freq(CLKSEL_40MHZ | CLKDIV_32) / freq < 0xFFFF)
mode = CLKSEL_40MHZ | CLKDIV_32;
else if (get_base_freq(CLKSEL_40MHZ | CLKDIV_64) / freq < 0xFFFF)
mode = CLKSEL_40MHZ | CLKDIV_64;
else if (get_base_freq(CLKSEL_40MHZ | CLKDIV_128) / freq < 0xFFFF)
mode = CLKSEL_40MHZ | CLKDIV_128;
else if (get_base_freq(CLKSEL_100KHZ | CLKDIV_1) / freq < 0xFFFF)
mode = CLKSEL_100KHZ | CLKDIV_1;
else if (get_base_freq(CLKSEL_100KHZ | CLKDIV_2) / freq < 0xFFFF)
mode = CLKSEL_100KHZ | CLKDIV_2;
else if (get_base_freq(CLKSEL_100KHZ | CLKDIV_4) / freq < 0xFFFF)
mode = CLKSEL_100KHZ | CLKDIV_4;
else if (get_base_freq(CLKSEL_100KHZ | CLKDIV_8) / freq < 0xFFFF)
mode = CLKSEL_100KHZ | CLKDIV_8;
else if (get_base_freq(CLKSEL_100KHZ | CLKDIV_16) / freq < 0xFFFF)
mode = CLKSEL_100KHZ | CLKDIV_16;
else if (get_base_freq(CLKSEL_100KHZ | CLKDIV_32) / freq < 0xFFFF)
mode = CLKSEL_100KHZ | CLKDIV_32;
else if (get_base_freq(CLKSEL_100KHZ | CLKDIV_64) / freq < 0xFFFF)
mode = CLKSEL_100KHZ | CLKDIV_64;
else if (get_base_freq(CLKSEL_100KHZ | CLKDIV_128) / freq < 0xFFFF)
mode = CLKSEL_100KHZ | CLKDIV_128;
else {
printf("Frequency out of range\n");
return 1;
}
base_freq = get_base_freq(mode);
// printf("mode=%d, base_freq=%d\n", mode, base_freq);
uint32_t duration = base_freq / freq;
uint32_t duration_h = duration * duty / 100;
uint32_t duration_l = duration * (100-duty) / 100;
pwm_raw(channel, mode, duration_h, duration_l);
return 0;
}
int main(int argc, char **argv)
{
const char *progname = argv[0];
if (argc < 3)
{
usage(progname);
exit(2);
}
uint8_t channel;
uint32_t freq;
uint8_t duty = 50;
if ((sscanf(argv[1], "%d", &channel) != 1) || (channel > 3))
{
fprintf(stderr, "Invalid channel number\n");
exit(1);
}
if (sscanf(argv[2], "%d", &freq) != 1)
{
fprintf(stderr, "Invalid frequency number\n");
exit(1);
}
if (argc >= 4)
{
if ((sscanf(argv[3], "%d", &duty) != 1) || (duty > 100))
{
fprintf(stderr, "Invalid duty number\n");
exit(1);
}
}
return pwm(channel, freq, duty);
}
|
