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
|
/* Copyright 2018 Raptor Engineering, LLC
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* 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 <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <fcntl.h>
#include <unistd.h>
#include <sys/stat.h>
#include <sys/mman.h>
#include <sys/resource.h>
#include <time.h>
#include <assert.h>
#include <inttypes.h>
#define SYSCTL_BASE 0x1e6e2000
#define SYSCTL_SCU88 0x88
#define SYSCTL_PWM7 7
#define PWM_BASE 0x1e786000
#define PTCR40 0x40
#define PTCR44 0x44
#define PTCR4C 0x4c
static void *sysctl_reg = NULL;
static void *pwm_reg = NULL;
static int mem_fd = 0;
// Adapted from ASpeed documentation
uint32_t get_ideal_divisor(double clk_in, double desired_clk) {
uint32_t div_low = 0;
uint32_t div_high = 0;
uint32_t retval = 0;
uint32_t tmpval2;
double tmpval;
double tmpval1;
if (desired_clk < 1) {
return 0;
}
double divisor = clk_in / desired_clk;
if (divisor < (0x1 << 8)) {
return 0;
}
tmpval = divisor;
tmpval2 = ((uint32_t)divisor) >> 8;
while ((div_high < (0x1 << 4)) && (tmpval2)) {
div_high++;
tmpval2 >>= 1;
}
if (!(div_high ^ (0x1 << 4))) {
div_high--;
}
tmpval1 = divisor / ((double)(1 << div_high));
tmpval = tmpval1;
while (tmpval >= (0x1 << 8));
retval |= div_low << 0;
retval |= div_high << 4;
retval |= ((uint32_t)(tmpval - 1)) << 8;
return retval;
}
inline void timespec_diff(struct timespec *start, struct timespec *stop, struct timespec *result) {
if ((stop->tv_nsec - start->tv_nsec) < 0) {
result->tv_sec = stop->tv_sec - start->tv_sec - 1;
result->tv_nsec = stop->tv_nsec - start->tv_nsec + 1000000000;
}
else {
result->tv_sec = stop->tv_sec - start->tv_sec;
result->tv_nsec = stop->tv_nsec - start->tv_nsec;
}
return;
}
void delay_for_interval(uint32_t interval) {
uint32_t abort_count = 0;
uint64_t delay_ns = interval * 1000;
struct timespec start_time = {.tv_sec=0,.tv_nsec=0};
struct timespec current_time = {.tv_sec=0,.tv_nsec=0};
struct timespec difference_time = {.tv_sec=0,.tv_nsec=0};
abort_count = 0;
while (clock_gettime(CLOCK_MONOTONIC_RAW, &start_time) < 0) {
abort_count++;
if (abort_count > 10) {
return;
}
}
while (1) {
current_time.tv_sec = 0;
current_time.tv_nsec = 0;
if (clock_gettime(CLOCK_MONOTONIC_RAW, ¤t_time) < 0) {
continue;
}
timespec_diff(&start_time, ¤t_time, &difference_time);
if (difference_time.tv_nsec > delay_ns) {
break;
}
}
if (difference_time.tv_nsec > (delay_ns * 2)) {
printf("OVERRAN timer (wanted %lld, got %lld, start: %lld.%lld end: %lld.%lld)\n", delay_ns, difference_time.tv_nsec, start_time.tv_sec, start_time.tv_nsec, current_time.tv_sec, current_time.tv_nsec);
}
}
int main(int argc, char* argv[]) {
uint32_t *offset = 0;
long unsigned int frequency_hz = 1000;
long unsigned int duration_ms = 100;
long unsigned int ast_clock_hz = 24000000;
if (argc > 1) {
frequency_hz = atoi(argv[1]);
}
if (argc > 2) {
duration_ms = atoi(argv[2]);
}
// Open physical memory device
if (!mem_fd) {
mem_fd = open("/dev/mem", O_RDWR | O_SYNC);
if (mem_fd < 0) {
perror("Unable to open /dev/mem");
exit(1);
}
}
// Map sytem control registers into virtual memory
sysctl_reg = mmap(NULL, getpagesize(), PROT_READ | PROT_WRITE, MAP_SHARED, mem_fd, SYSCTL_BASE);
if (sysctl_reg == MAP_FAILED) {
perror("Unable to map system control register memory");
exit(-1);
}
// Map PWM control registers into virtual memory
pwm_reg = mmap(NULL, getpagesize(), PROT_READ | PROT_WRITE, MAP_SHARED, mem_fd, PWM_BASE);
if (pwm_reg == MAP_FAILED) {
perror("Unable to map PWM register memory");
exit(-1);
}
// Elevate priority
if (setpriority(PRIO_PROCESS, 0, -20) < 0) {
perror("Unable to set priority");
exit(-1);
}
// Set GPIO N7 to PWM7
offset = sysctl_reg + SYSCTL_SCU88;
*offset |= (0x1 << SYSCTL_PWM7);
// Set H port to type "O"
offset = pwm_reg + PTCR40;
*offset &= ~(0x1 << 15);
*offset |= 0x1 << 7;
// Set PWM H rising / falling points
offset = pwm_reg + PTCR4C;
*offset &= ~0xffff0000;
*offset |= 0x8000 << 16;
// Set PWM H divisor and period
uint32_t divisor_register = get_ideal_divisor(ast_clock_hz, frequency_hz);
offset = pwm_reg + PTCR44;
*offset = divisor_register;
// Enable H port PWM output
offset = pwm_reg + PTCR40;
*offset |= 0x1 << 11;
// Ideally usleep() would be used here, but the platform timers are buggy...
delay_for_interval(duration_ms * 1000);
// Disable H port PWM output
offset = pwm_reg + PTCR40;
*offset &= ~(0x1 << 11);
return 0;
}
|
