-
Notifications
You must be signed in to change notification settings - Fork 1
/
pwm_dma_eti.cpp
539 lines (467 loc) · 19.1 KB
/
pwm_dma_eti.cpp
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
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
//Inspired by https://github.com/Wallacoloo/Raspberry-Pi-DMA-Example/blob/master/dma-gpio.c
//and http://www.icrobotics.co.uk/wiki/index.php/Turning_the_Raspberry_Pi_Into_an_FM_Transmitter
//and https://github.com/hzeller/rpi-gpio-dma-demo/blob/master/gpio-dma-test.c
// 0 1
//0 0 X
//1 X X
//Option 1:
// 0 1
//0 0 +
//1 ? -
//Option 2:
// 0 1
//0 0 -
//1 ? +
//Option 2:
// 0 1
//0 0 +
//1 - ?
#include <sys/time.h>
//#include <sys/mman.h> //for mmap
#include <stdio.h>
#include <signal.h> //for sigaction
#include <stdlib.h> //for exit, valloc
#include <unistd.h> //for NULL, lseek
#include <fcntl.h> //for file opening
#include <string.h> //for memset
#include <errno.h> //for errno
#include <pthread.h> //for pthread_setschedparam
#include <assert.h>
#include <zmq.h>
#include <deque>
#define RPI_V2
#include "hw-addresses.h"
#include "pwm_dma_eti_sys.h"
#include "odr-dabOutput.h"
#include "hdb3encodechain.hpp"
#include "uncached_memblock.h"
#include "zmqreader.h"
/************/
/* Settings */
/************/
#define SCHED_PRIORITY 30 //Linux scheduler priority. Higher = more realtime
#define DMA_CHANNEL (6)
#define CLOCK_DIVI (122)
#define CLOCK_DIVF (288)
#define STATUS_GPIO (27)
//map a physical address into our virtual address space. memfd is the file descriptor for /dev/mem
volatile uint32_t* mapPeripheral(int memfd, int addr) {
///dev/mem behaves as a file. We need to map that file into memory:
//NULL = virtual address of mapping is chosen by kernel.
//PAGE_SIZE = map 1 page.
//PROT_READ|PROT_WRITE means give us read and write priveliges to the memory
//MAP_SHARED means updates to the mapped memory should be written back to the file & shared with other processes
//memfd = /dev/mem file descriptor
//addr = offset in file to map
void *mapped = mmap(NULL, PAGE_SIZE, PROT_READ|PROT_WRITE, MAP_SHARED, memfd, addr);
//now, *mapped = memory at physical address of addr.
if (mapped == MAP_FAILED) {
printf("failed to map memory (did you remember to run as root?)\n");
exit(1);
} else {
printf("mapped: %p\n", mapped);
}
return (volatile uint32_t*)mapped;
}
volatile uint32_t *gpio;
struct DmaChannelHeader *dmaHeader; //must be global for cleanup()
struct PwmHeader *pwmHeader;
struct ClockManagerHeader *cmHeader;
void setSchedPriority(int priority) {
//In order to get the best timing at a decent queue size, we want the kernel to avoid interrupting us for long durations.
//This is done by giving our process a high priority. Note, must run as super-user for this to work.
struct sched_param sp;
sp.sched_priority=priority;
int ret;
if ((ret = pthread_setschedparam(pthread_self(), SCHED_FIFO, &sp))) {
printf("Warning: pthread_setschedparam (increase thread priority) returned non-zero: %i\n", ret);
}
}
void writeBitmasked(volatile uint32_t *dest, uint32_t mask, uint32_t value) {
//set bits designated by (mask) at the address (dest) to (value), without affecting the other bits
//eg if x = 0b11001100
// writeBitmasked(&x, 0b00000110, 0b11110011),
// then x now = 0b11001110
uint32_t cur = *dest;
uint32_t new_dest = (cur & (~mask)) | (value & mask);
*dest = new_dest;
*dest = new_dest; //best to be safe when crossing memory boundaries
}
void logDmaChannelHeader(struct DmaChannelHeader *h) {
#if 0
printf("Dma Ch Header:\n CS: 0x%08x\n CONBLK_AD: 0x%08x\n TI: 0x%08x\n SOURCE_AD: 0x%08x\n DEST_AD: 0x%08x\n TXFR_LEN: %u\n STRIDE: 0x%08x\n NEXTCONBK: 0x%08x\n DEBUG: 0x%08x\n", h->CS, h->CONBLK_AD, h->TI, h->SOURCE_AD, h->DEST_AD, h->TXFR_LEN, h->STRIDE, h->NEXTCONBK, h->DEBUG);
#else
uint32_t CS = h->CS;
printf("Dma Ch Header:\n CS: 0x%08x\n", CS);
printf(" %1d RESET\n", (CS >> 31) & 1);
printf(" %1d ABORT\n", (CS >> 30) & 1);
printf(" %1d DISDEBUG\n", (CS >> 29) & 1);
printf(" %1d WAIT_FOR_OUTSTANDING_WRITES\n", (CS >> 28) & 1);
printf(" PANIC_PRIORITY: %d\n", (CS >> 20) & 0xF);
printf(" PRIORITY: %d\n", (CS >> 16) & 0xF);
printf(" %1d ERROR\n", (CS >> 8) & 1);
printf(" %1d WAITING_FOR_OUTSTANDING_WRITES\n", (CS >> 6) & 1);
printf(" %1d DREQ_STOPS_DMA\n", (CS >> 5) & 1);
printf(" %1d PAUSED\n", (CS >> 4) & 1);
printf(" %1d DREQ\n", (CS >> 3) & 1);
printf(" %1d ACTIVE\n", (CS >> 0) & 1);
printf(" CONBLK_AD: 0x%08x\n TI: 0x%08x\n SOURCE_AD: 0x%08x\n DEST_AD: 0x%08x\n TXFR_LEN: %u\n STRIDE: 0x%08x\n NEXTCONBK: 0x%08x\n", h->CONBLK_AD, h->TI, h->SOURCE_AD, h->DEST_AD, h->TXFR_LEN, h->STRIDE, h->NEXTCONBK);
uint32_t DEBUG = h->DEBUG;
printf(" DEBUG: 0x%08x\n", DEBUG);
printf(" %1d LITE\n", (DEBUG >> 28) & 1);
printf(" VERSION: %d\n", (DEBUG >> 25) & 0x7);
printf(" DMA_STATE: %d\n", (DEBUG >> 16) & 0x1FF);
printf(" DMA_ID: %d\n", (DEBUG >> 8) & 0xFF);
printf(" OUTSTANDING_WRITES: %d\n", (DEBUG >> 4) & 0xF);
printf(" %1d READ_ERROR\n", (DEBUG >> 2) & 1);
printf(" %1d FIFO_ERROR\n", (DEBUG >> 1) & 1);
printf(" %1d READ_LAST_NOT_SET_ERROR\n", (DEBUG >> 0) & 1);
#endif
}
void logDmaControlBlock(struct DmaControlBlock *b) {
printf("Dma Control Block:\n TI: 0x%08x\n SOURCE_AD: 0x%08x\n DEST_AD: 0x%08x\n TXFR_LEN: 0x%08x\n STRIDE: 0x%08x\n NEXTCONBK: 0x%08x\n unused: 0x%08x %08x\n", b->TI, b->SOURCE_AD, b->DEST_AD, b->TXFR_LEN, b->STRIDE, b->NEXTCONBK, b->_reserved[0], b->_reserved[1]);
}
volatile int stop_program = 0;
void *zmq_ctx;
ZMQReader *zmqreader;
void logPWMState(uint32_t pwmState) {
printf("PWM-STA:");
if (pwmState & (1 << 0)) printf(" FULL1");
if (pwmState & (1 << 1)) printf(" EMPT1");
if (pwmState & (1 << 2)) printf(" WERR1");
if (pwmState & (1 << 3)) printf(" RERR1");
if (pwmState & (1 << 4)) printf(" GAPO1");
if (pwmState & (1 << 5)) printf(" GAPO2");
if (pwmState & (1 << 6)) printf(" GAPO3");
if (pwmState & (1 << 7)) printf(" GAPO4");
if (pwmState & (1 << 8)) printf(" BERR");
if (pwmState & (1 << 9)) printf(" STA1");
if (pwmState & (1 << 10)) printf(" STA2");
if (pwmState & (1 << 11)) printf(" STA3");
if (pwmState & (1 << 12)) printf(" STA4");
printf("\n");
}
class EncodedHDB3WordConsumer {
protected:
uint64_t frame_processed = 0;
uint64_t frame_send = 0;
uint64_t gap_counter = 0;
int previous_cb;
bool hadPaddingBefore = false;
int current_frame = -1;
int current_frame_index;
static constexpr int BUFFER_COUNT = 40;
static constexpr int ETI_FRAME_SIZE = 6144;
int last_frame = 0;
struct timeval start_time;
//We encode the bits at double the frequency and have 2 channels, so each ETI-Frame is encoded to 4 times its size in bytes.
//So each 2 bytes in the eti frame will be encoded to 2 uint32_t
static constexpr size_t BUFFER_BYTES_PER_FRAME = ETI_FRAME_SIZE * 2 * 2;
static constexpr size_t BUFFER_WORDS_PER_FRAME = ETI_FRAME_SIZE;
UncachedMemBlock srcPage;
uint32_t *srcArray;
UncachedMemBlock cbPage;
struct DmaControlBlock *cbArr;
struct DmaControlBlock *cbArrPadding;
struct ClockManagerClockHeader *pwmClock;
void setupClock() {
pwmClock = &cmHeader->PWM;
printf("setting up clock for pwm\n");
//Setup Clock for PWM
pwmClock->CTL = CM_CTL_PASSWD | ((pwmClock->CTL)&(~CM_CTL_ENAB)); //disable clock
do {} while (pwmClock->CTL & CM_CTL_BUSY); //wait for clock to deactivate
pwmClock->DIV = CM_DIV_PASSWD | CM_DIV_DIVI(CLOCK_DIVI) | CM_DIV_DIVF(CLOCK_DIVF); //configure clock divider (running at 500MHz undivided)
pwmClock->CTL = CM_CTL_PASSWD | CM_CTL_SRC_PLLD | CM_CTL_MASH(1); //source 500MHz base clock, MASH1.
pwmClock->CTL = CM_CTL_PASSWD | CM_CTL_SRC_PLLD | CM_CTL_ENAB | CM_CTL_MASH(1); //enable clock
do {} while ((pwmClock->CTL & CM_CTL_BUSY) == 0); //wait for clock to activate
}
void setupPWM() {
printf("Shutting down PWM\n");
pwmHeader->DMAC = 0; //disable DMA
pwmHeader->CTL = PWM_CTL_CLRFIFO; //clear pwm
usleep(100);
while(pwmHeader->STA & (0x3<<9)) { printf("PWM still running..."); usleep(100);}
pwmHeader->STA = PWM_STA_ERRS; //clear PWM errors
usleep(100);
printf("Setting up PWM\n");
pwmHeader->DMAC = PWM_DMAC_EN | PWM_DMAC_DREQ(4) | PWM_DMAC_PANIC(4); //DREQ is activated at queue < PWM_FIFO_SIZE
pwmHeader->RNG1 = 32;
pwmHeader->RNG2 = 32;
pwmHeader->CTL = PWM_CTL_ENABLE1 | PWM_CTL_USEFIFO1 | PWM_CTL_SERIAL1 |
PWM_CTL_ENABLE2 | PWM_CTL_USEFIFO2 | PWM_CTL_SERIAL2 ;
printf("Setting up GPIO18 + 13 for PWM0 and PWM1\n");
SET_GPIO_ALT(18,5); //GPIO18 = PIN12
SET_GPIO_ALT(13,0); //GPIO13 = PIN33
}
void setupDMA() {
srcArray = (uint32_t*)srcPage.mem;
memset(srcPage.mem, 0, srcPage.size);
cbArr = (struct DmaControlBlock*)cbPage.mem;
cbArrPadding = ((struct DmaControlBlock*)cbPage.mem) + BUFFER_COUNT;
for (int i = 0; i < BUFFER_COUNT; i++) {
cbArr[i].TI = DMA_CB_TI_PERMAP_PWM | DMA_CB_TI_DEST_DREQ | DMA_CB_TI_SRC_INC | DMA_CB_TI_WAIT_RESP;
cbArr[i].SOURCE_AD = srcPage.to_physical(((uint8_t*)srcArray) + i * BUFFER_BYTES_PER_FRAME);
cbArr[i].DEST_AD = PWM_BASE_BUS + PWM_FIF1; //write to the FIFO
cbArr[i].TXFR_LEN = BUFFER_BYTES_PER_FRAME;
cbArr[i].STRIDE = 0;
cbArr[i].NEXTCONBK = cbPage.to_physical(cbArr+((i+1) % BUFFER_COUNT));
cbArrPadding[i].TI = DMA_CB_TI_PERMAP_PWM | DMA_CB_TI_DEST_DREQ | DMA_CB_TI_WAIT_RESP;
//cbArr[i].SOURCE_AD will be filled in separately
cbArrPadding[i].DEST_AD = PWM_BASE_BUS + PWM_FIF1;
//cbArr[i].TXFR_LEN = BUFFER_BYTES_PER_FRAME;
cbArr[i].STRIDE = 0;
//cbArr[i].NEXTCONBK = cbPage.to_physical(cbArr+((i+1) % BUFFER_COUNT));
}
printf("Previous DMA header:\n");
logDmaChannelHeader(dmaHeader);
printf("Stopping DMA\n");
dmaHeader->NEXTCONBK = 0;
dmaHeader->CS |= DMA_CS_ABORT; //make sure to disable dma first.
usleep(100); //give time for the abort command to be handled.
dmaHeader->CS = DMA_CS_RESET;
usleep(1100);
writeBitmasked(&dmaHeader->CS, DMA_CS_END, DMA_CS_END); //clear the end flag
usleep(100);
dmaHeader->DEBUG = DMA_DEBUG_READ_ERROR | DMA_DEBUG_FIFO_ERROR | DMA_DEBUG_READ_LAST_NOT_SET_ERROR; // clear debug error flags
uint32_t firstAddr = cbPage.to_physical(cbArr);
printf("starting DMA @ CONBLK_AD=0x%08x\n", firstAddr);
dmaHeader->CONBLK_AD = firstAddr; //(uint32_t)physCbPage + ((void*)cbArr - virtCbPage); //we have to point it to the PHYSICAL address of the control block (cb1)
dmaHeader->CS = DMA_CS_PRIORITY(7) | DMA_CS_PANIC_PRIORITY(15) | DMA_CS_WAIT_FOR_OUTSTANDING_WRITES; //high priority (max is 7)
dmaHeader->CS = DMA_CS_PRIORITY(7) | DMA_CS_PANIC_PRIORITY(15) | DMA_CS_WAIT_FOR_OUTSTANDING_WRITES | DMA_CS_ACTIVE; //activate DMA.
}
inline void advanceCommandBlock(bool didPadding) {
if(hadPaddingBefore) {
assert(cbArrPadding[previous_cb].NEXTCONBK == 0);
cbArrPadding[previous_cb].NEXTCONBK = cbPage.to_physical(&cbArr[current_frame]);;
} else {
assert(cbArr[previous_cb].NEXTCONBK == 0);
cbArr[previous_cb].NEXTCONBK = cbPage.to_physical(&cbArr[current_frame]);;
}
previous_cb = current_frame;
current_frame = (current_frame + 1) % BUFFER_COUNT;
current_frame_index = 0;
frame_processed += 1;
hadPaddingBefore = didPadding;
}
inline void waitForCurrentFrameToEnd() {
//usleep(dmaHeader->TXFR_LEN * 24 * 1000 / BUFFER_BYTES_PER_FRAME);
usleep(24 * 1000 * 2);
}
inline int getCurrentDMAFrame() {
uint32_t dma_pos = (dmaHeader->CONBLK_AD - cbPage.bus_addr) / sizeof(struct DmaControlBlock);
assert(dma_pos < 2 * BUFFER_COUNT && dma_pos >= 0);
return (dma_pos % BUFFER_COUNT);
}
public:
EncodedHDB3WordConsumer() : srcPage(BUFFER_COUNT * BUFFER_BYTES_PER_FRAME), cbPage(BUFFER_COUNT * 2 * sizeof(struct DmaControlBlock)) {
setupClock();
setupPWM();
setupDMA();
}
void consumeEncodedHdb3(uint32_t out_p, uint32_t out_m) {
static struct timeval last_info;
if (current_frame < 0) {
assert(!hadPaddingBefore);
current_frame = getCurrentDMAFrame();
printf("Starting... %d\n", current_frame);
previous_cb = (BUFFER_COUNT + current_frame - 1) % BUFFER_COUNT;
cbArr[previous_cb].NEXTCONBK = 0;
waitForCurrentFrameToEnd();
printf("Frame ended...\n");
while(current_frame == getCurrentDMAFrame()) { printf("Need to wait a little longer %d\n", current_frame);}
gettimeofday(&start_time, NULL);
last_frame = getCurrentDMAFrame();
current_frame_index = 0;
cbArr[current_frame].NEXTCONBK = 0;
} else if (current_frame == getCurrentDMAFrame()) {
//Need to wait until we can write into this DMA-Block
assert(current_frame_index == 0);
zmqreader->tryReceive();
waitForCurrentFrameToEnd();
if (current_frame == getCurrentDMAFrame()) {
logDmaChannelHeader(dmaHeader);
printf("Need to wait a little longer2: %d\n", current_frame);
while(current_frame == getCurrentDMAFrame()) {usleep(100);}
}
current_frame_index = 0;
cbArr[current_frame].NEXTCONBK = 0;
}
int dma_frame = getCurrentDMAFrame();
if (last_frame != dma_frame) {
frame_send += (BUFFER_COUNT + dma_frame - last_frame) % BUFFER_COUNT;
last_frame = dma_frame;
}
srcArray[current_frame * BUFFER_WORDS_PER_FRAME + current_frame_index] = out_p;
srcArray[current_frame * BUFFER_WORDS_PER_FRAME + current_frame_index + 1] = out_m;
current_frame_index += 2;
if (current_frame_index >= 0 && (unsigned)current_frame_index >= BUFFER_WORDS_PER_FRAME) {
cbArr[current_frame].NEXTCONBK = 0;
//Full size frame without padding
advanceCommandBlock(false);
}
struct timeval now;
gettimeofday(&now, NULL);
//time_t now = time(0);
if ((pwmHeader->STA & (0xF0)) > 0) {
//printf("GAP!!! %d\n", (pwmHeader->STA & (0xF0)) >> 4);
pwmHeader->STA = 0xF0;
gap_counter++;
//last_gap = now;
}
if (last_info.tv_sec != now.tv_sec) {
double diff = (now.tv_sec - start_time.tv_sec) + (now.tv_usec - start_time.tv_usec) / 1000000.0;
last_info = now;
printf("Runtime: %7lds E: %10.3lfB/s S: %10.3lf gaps: %llu buffer: %dms zmq: %dx4\n",
(long)diff,
frame_processed * 6144.0/ (diff),
frame_send * 6144.0 / (diff),
gap_counter,
((BUFFER_COUNT + current_frame - getCurrentDMAFrame()) % BUFFER_COUNT ) * 24 + dmaHeader->TXFR_LEN * 24 / BUFFER_BYTES_PER_FRAME,
zmqreader->bufferSize()
);
if (dmaHeader->CONBLK_AD == 0 || dmaHeader->NEXTCONBK == 0) {
printf("MAX-GAP!\n");
stop_program = 1;
}
}
}
void consumePadding(int repeats, uint32_t padding_out_p, uint32_t padding_out_m) {
if (repeats >= 2) {
assert(current_frame >= 0);
//Each repeat of the padding byte is 2 Bytes.
assert(6144 - current_frame_index == repeats * 2);
assert(current_frame_index >= 0 && (unsigned)current_frame_index + 4 < BUFFER_WORDS_PER_FRAME);
//Prepare for 128-bit-read.
srcArray[current_frame * BUFFER_WORDS_PER_FRAME + current_frame_index] = padding_out_p;
srcArray[current_frame * BUFFER_WORDS_PER_FRAME + current_frame_index + 1] = padding_out_m;
srcArray[current_frame * BUFFER_WORDS_PER_FRAME + current_frame_index + 2] = padding_out_p;
srcArray[current_frame * BUFFER_WORDS_PER_FRAME + current_frame_index + 3] = padding_out_m;
cbArrPadding[current_frame].SOURCE_AD =
srcPage.to_physical(
((uint8_t*)srcArray) + current_frame * BUFFER_BYTES_PER_FRAME + sizeof(uint32_t) * current_frame_index
);
cbArrPadding[current_frame].NEXTCONBK = 0;
//Previous-CB --> [Previous CB Padding] --> This CB --> This CB Padding
cbArrPadding[current_frame].TXFR_LEN = repeats * sizeof(uint32_t) * 2;
//This CB is not the full length
cbArr[current_frame].TXFR_LEN = current_frame_index * sizeof(uint32_t);
assert(cbArr[current_frame].TXFR_LEN + cbArrPadding[current_frame].TXFR_LEN == 6144 * 4);
cbArr[current_frame].NEXTCONBK = cbPage.to_physical(&cbArrPadding[current_frame]);
advanceCommandBlock(true);
} else if (repeats == 1) {
consumeEncodedHdb3(padding_out_p, padding_out_m);
}
}
~EncodedHDB3WordConsumer() {
for(int i = 0; i < BUFFER_COUNT; i++) {
cbArr[i].NEXTCONBK = 0;
cbArrPadding[i].NEXTCONBK = 0;
}
dmaHeader->NEXTCONBK = 0;
dmaHeader->CS |= DMA_CS_ABORT;
usleep(100); //give time for the abort command to be handled.
dmaHeader->CS = DMA_CS_RESET;
pwmHeader->CTL = 0;
pwmClock->CTL = CM_CTL_PASSWD | ((pwmClock->CTL)&(~CM_CTL_ENAB)); //disable clock
do {} while (pwmClock->CTL & CM_CTL_BUSY); //wait for clock to deactivate
}
};
typedef ETIFrameConsumer<ETIFrameBitConsumer<HDB3TertiaryConsumer<EncodedHDB3WordConsumer>>> EncodingChain;
EncodingChain *chain;
void cleanup() {
printf("Cleanup\n");
stop_program = 1;
GPIO_CLR = 1<<STATUS_GPIO;
zmqreader->keep_running = false;
delete zmqreader;
if (zmq_ctx) {
printf("Terminating ZMQ-Context\n");
zmq_term(zmq_ctx);
}
delete chain;
//disable DMA. Otherwise, it will continue to run in the background, potentially overwriting future user data.
if(dmaHeader) {
printf("Stopping DMA\n");
writeBitmasked(&dmaHeader->CS, DMA_CS_ACTIVE, 0);
usleep(100);
writeBitmasked(&dmaHeader->CS, DMA_CS_RESET, DMA_CS_RESET);
}
printf("Stopping PWM\n");
pwmHeader->CTL = 0;
}
void cleanupAndExit(int sig) {
cleanup();
printf("Exiting with error; caught signal: %i\n", sig);
exit(1);
}
int main(int argc, const char *argv[]) {
if (argc != 2) {
fprintf(stderr, "Missing Argument.\nUsage: %s [ZMQ-Endpoint]\n", argv[0]);
return 1;
}
//Setup peripherals
volatile uint32_t *dmaBaseMem, *pwmBaseMem, *clockBaseMem;
//emergency clean-up:
for (int i = 0; i < 64; i++) { //catch all signals (like ctrl+c, ctrl+z, ...) to ensure DMA is disabled
struct sigaction sa;
memset(&sa, 0, sizeof(sa));
sa.sa_handler = cleanupAndExit;
sigaction(i, &sa, NULL);
}
setSchedPriority(SCHED_PRIORITY);
assert(argc == 2);
zmq_ctx = zmq_init(1);
if (!zmq_ctx) {
fprintf(stderr, "%s\n", "Error initializing zmq_ctx.");
exit(1);
}
//First, open the linux device, /dev/mem
//dev/mem provides access to the physical memory of the entire processor+ram
//This is needed because Linux uses virtual memory, thus the process's memory at 0x00000000 will NOT have the same contents as the physical memory at 0x00000000
int memfd = open("/dev/mem", O_RDWR | O_SYNC);
if (memfd < 0) {
printf("Failed to open /dev/mem (did you remember to run as root?)\n");
exit(1);
}
//now map /dev/mem into memory, but only map specific peripheral sections:
gpio = mapPeripheral(memfd, GPIO_BASE);
dmaBaseMem = mapPeripheral(memfd, DMA_BASE);
dmaHeader = (struct DmaChannelHeader*)(dmaBaseMem + DMACH(DMA_CHANNEL)/4); //must divide by 4, as dmaBaseMem is uint32_t*
pwmBaseMem = mapPeripheral(memfd, PWM_BASE);
pwmHeader = (struct PwmHeader*)(pwmBaseMem);
//timerBaseMem = mapPeripheral(memfd, TIMER_BASE);
clockBaseMem = mapPeripheral(memfd, CLOCK_BASE);
cmHeader = (struct ClockManagerHeader*)(clockBaseMem + 0x70/4);
zmqreader = new ZMQReader(zmq_ctx, argv[1]);
chain = new EncodingChain();
INP_GPIO(STATUS_GPIO);
OUT_GPIO(STATUS_GPIO);
GPIO_CLR = 1<<STATUS_GPIO;
int frame_count = 0;
bool blink_status = 0;
zmq_dab_message_t *dab_msg = zmqreader->getNextMessage();
while(!stop_program) {
int frame_offset = 0;
for (int frame = 0; frame < NUM_FRAMES_PER_ZMQ_MESSAGE; frame++) {
chain->consumeETIFrame(dab_msg->buf + frame_offset, dab_msg->buflen[frame]);
frame_offset += dab_msg->buflen[frame];
frame_count += 1;
if (frame_count >= 42) {
frame_count = 0;
if (blink_status) {
GPIO_CLR = 1<<STATUS_GPIO;
blink_status = false;
} else {
GPIO_SET = 1<<STATUS_GPIO;
blink_status = true;
}
}
}
dab_msg = zmqreader->getNextMessage();
}
GPIO_CLR = 1<<STATUS_GPIO;
printf("Cleanup...\n");
pwmHeader->CTL = 0;
cleanup();
return 0;
}