mirror
/
fm_transmitter
kopia lustrzana https://github.com/markondej/fm_transmitter
1
0
Forkuj 0
Kod Zgłoszenia Projekty Wydania Wiki Aktywność

Simplified code

pull/70/head
Marcin Kondej 2019-01-07 14:56:06 +01:00
rodzic a08ff1446b
commit b7b13ad836
2 zmienionych plików z 81 dodań i 60 usunięć
Pokaż statystyki Ściągnij plik aktualizacji Ściągnij plik porównania Expand all files Collapse all files

127
transmitter.cpp
Wyświetl plik

@ -117,6 +117,7 @@ bool Transmitter::preserveCarrier = false;
void *Transmitter::peripherals = NULL; void *Transmitter::peripherals = NULL;
Transmitter::Transmitter() Transmitter::Transmitter()
: memSize(0)
{ {
int memFd; int memFd;
if ((memFd = open("/dev/mem", O_RDWR | O_SYNC)) < 0) { if ((memFd = open("/dev/mem", O_RDWR | O_SYNC)) < 0) {
@ -141,6 +142,42 @@ Transmitter &Transmitter::getInstance()
return instance; return instance;
} }
bool Transmitter::allocateMemory(unsigned size)
{
if (memSize) {
return false;
}
mBoxFd = mbox_open();
memSize = size;
if (memSize % PAGE_SIZE) {
memSize = (memSize / PAGE_SIZE + 1) * PAGE_SIZE;
}
memHandle = mem_alloc(mBoxFd, size, PAGE_SIZE, (bcm_host_get_peripheral_address() == BCM2835_PERI_VIRT_BASE) ? BCM2835_MEM_FLAG : BCM2837_MEM_FLAG);
if (!memHandle) {
mbox_close(mBoxFd);
memSize = 0;
return false;
}
memAddress = mem_lock(mBoxFd, memHandle);
memAllocated = mapmem(memAddress & ~0xC0000000, memSize);
return true;
}
void Transmitter::freeMemory()
{
unmapmem(memAllocated, memSize);
mem_unlock(mBoxFd, memHandle);
mem_free(mBoxFd, memHandle);
mbox_close(mBoxFd);
memSize = 0;
}
unsigned Transmitter::getAddress(volatile void *object)
{
return (memSize) ? memAddress + ((unsigned)object - (unsigned)memAllocated) : 0x00000000;
}
void Transmitter::play(WaveReader &reader, double frequency, unsigned char dmaChannel, bool preserveCarrierOnExit) void Transmitter::play(WaveReader &reader, double frequency, unsigned char dmaChannel, bool preserveCarrierOnExit)
{ {
if (transmitting) { if (transmitting) {
@ -159,6 +196,8 @@ void Transmitter::play(WaveReader &reader, double frequency, unsigned char dmaCh
bool eof = samples->size() < bufferSize; bool eof = samples->size() < bufferSize;
unsigned clockDivisor = (unsigned)((500 << 12) / frequency + 0.5); unsigned clockDivisor = (unsigned)((500 << 12) / frequency + 0.5);
bool isError = false;
string errorMessage;
if (dmaChannel != 0xFF) { if (dmaChannel != 0xFF) {
if (dmaChannel > 15) { if (dmaChannel > 15) {
@ -166,39 +205,30 @@ void Transmitter::play(WaveReader &reader, double frequency, unsigned char dmaCh
throw ErrorReporter("DMA channel number out of range (0 - 15)"); throw ErrorReporter("DMA channel number out of range (0 - 15)");
} }
int mbFd = mbox_open(); if (!allocateMemory(sizeof(unsigned) * ((bufferSize << 1) + 1) + sizeof(DMAControllBlock) * (bufferSize << 1))) {
unsigned reqMemSize = sizeof(unsigned) * ((bufferSize << 1) + 1) + sizeof(DMAControllBlock) * (bufferSize << 1);
if (reqMemSize % PAGE_SIZE) {
reqMemSize = (reqMemSize / PAGE_SIZE + 1) * PAGE_SIZE;
}
unsigned memHandle = mem_alloc(mbFd, reqMemSize, PAGE_SIZE, (bcm_host_get_peripheral_address() == BCM2835_PERI_VIRT_BASE) ? BCM2835_MEM_FLAG : BCM2837_MEM_FLAG);
if (!memHandle) {
delete samples; delete samples;
throw ErrorReporter("Cannot allocate memory"); throw ErrorReporter("Cannot allocate memory");
} }
unsigned physAddr = mem_lock(mbFd, memHandle);
void *virtAddr = mapmem(physAddr & ~0xC0000000, reqMemSize);
ClockRegisters *clk0 = (ClockRegisters *)getPeripheral(CLK0_BASE_OFFSET); volatile ClockRegisters *clk0 = (ClockRegisters *)getPeripheral(CLK0_BASE_OFFSET);
unsigned *gpio = (unsigned *)getPeripheral(GPIO_BASE_OFFSET); volatile unsigned *gpio = (unsigned *)getPeripheral(GPIO_BASE_OFFSET);
if (!clockInitialized) { if (!clockInitialized) {
clk0->ctl = (0x5A << 24) | 0x06; clk0->ctl = (0x5A << 24) | 0x06;
usleep(1000); usleep(1000);
clk0->div = (0x5A << 24) | clockDivisor; clk0->div = (0x5A << 24) | clockDivisor;
clk0->ctl = (0x5A << 24) | (0x01 << 9) | (0x01 << 4) | 0x06; clk0->ctl = (0x5A << 24) | (0x01 << 9) | (0x01 << 4) | 0x06;
*gpio = (*(volatile unsigned *)gpio & 0xFFFF8FFF) | (0x01 << 14); *gpio = (*gpio & 0xFFFF8FFF) | (0x01 << 14);
clockInitialized = true; clockInitialized = true;
} }
ClockRegisters *pwmClk = (ClockRegisters *)getPeripheral(PWMCLK_BASE_OFFSET); volatile ClockRegisters *pwmClk = (ClockRegisters *)getPeripheral(PWMCLK_BASE_OFFSET);
float pwmClkFreq = PWM_WRITES_PER_SAMPLE * PWM_CHANNEL_RANGE * header.sampleRate / 1000000; float pwmClkFreq = PWM_WRITES_PER_SAMPLE * PWM_CHANNEL_RANGE * header.sampleRate / 1000000;
pwmClk->ctl = (0x5A << 24) | 0x06; pwmClk->ctl = (0x5A << 24) | 0x06;
usleep(1000); usleep(1000);
pwmClk->div = (0x5A << 24) | (unsigned)((500 << 12) / pwmClkFreq); pwmClk->div = (0x5A << 24) | (unsigned)((500 << 12) / pwmClkFreq);
pwmClk->ctl = (0x5A << 24) | (0x01 << 4) | 0x06; pwmClk->ctl = (0x5A << 24) | (0x01 << 4) | 0x06;
PWMRegisters *pwm = (PWMRegisters *)getPeripheral(PWM_BASE_OFFSET); volatile PWMRegisters *pwm = (PWMRegisters *)getPeripheral(PWM_BASE_OFFSET);
pwm->ctl = 0x00; pwm->ctl = 0x00;
usleep(1000); usleep(1000);
pwm->status = 0x01FC; pwm->status = 0x01FC;
@ -208,15 +238,15 @@ void Transmitter::play(WaveReader &reader, double frequency, unsigned char dmaCh
pwm->dmaConf = (0x01 << 31) | 0x0707; pwm->dmaConf = (0x01 << 31) | 0x0707;
pwm->ctl = (0x01 << 5) | (0x01 << 2) | 0x01; pwm->ctl = (0x01 << 5) | (0x01 << 2) | 0x01;
float value;
unsigned i, cbIndex = 0;
#ifndef NO_PREEMP #ifndef NO_PREEMP
PreEmp preEmp(header.sampleRate); PreEmp preEmp(header.sampleRate);
#endif #endif
float value;
unsigned i, cbIndex = 0;
DMAControllBlock *dmaCb = (DMAControllBlock *)(unsigned *)virtAddr; volatile DMAControllBlock *dmaCb = (DMAControllBlock *)memAllocated;
unsigned *clkDiv = (unsigned *)virtAddr + ((sizeof(DMAControllBlock) / sizeof(unsigned)) << 1) * bufferSize; volatile unsigned *clkDiv = (unsigned *)memAllocated + ((sizeof(DMAControllBlock) / sizeof(unsigned)) << 1) * bufferSize;
unsigned *pwmFifoData = (unsigned *)virtAddr + (((sizeof(DMAControllBlock) / sizeof(unsigned)) << 1) + 1) * bufferSize; volatile unsigned *pwmFifoData = (unsigned *)memAllocated + (((sizeof(DMAControllBlock) / sizeof(unsigned)) << 1) + 1) * bufferSize;
for (i = 0; i < bufferSize; i++) { for (i = 0; i < bufferSize; i++) {
value = (*samples)[i].getMonoValue(); value = (*samples)[i].getMonoValue();
#ifndef NO_PREEMP #ifndef NO_PREEMP
@ -224,35 +254,32 @@ void Transmitter::play(WaveReader &reader, double frequency, unsigned char dmaCh
#endif #endif
clkDiv[i] = (0x5A << 24) | (clockDivisor - (int)round(value * 16.0)); clkDiv[i] = (0x5A << 24) | (clockDivisor - (int)round(value * 16.0));
dmaCb[cbIndex].transferInfo = (0x01 << 26) | (0x01 << 3); dmaCb[cbIndex].transferInfo = (0x01 << 26) | (0x01 << 3);
dmaCb[cbIndex].srcAddress = physAddr + ((unsigned)&clkDiv[i] - (unsigned)virtAddr); dmaCb[cbIndex].srcAddress = getAddress(&clkDiv[i]);
dmaCb[cbIndex].dstAddress = PERIPHERALS_PHYS_BASE | (CLK0_BASE_OFFSET + 0x04); dmaCb[cbIndex].dstAddress = PERIPHERALS_PHYS_BASE | (CLK0_BASE_OFFSET + 0x04);
dmaCb[cbIndex].transferLen = sizeof(unsigned); dmaCb[cbIndex].transferLen = sizeof(unsigned);
dmaCb[cbIndex].stride = 0; dmaCb[cbIndex].stride = 0;
dmaCb[cbIndex].nextCbAddress = physAddr + ((unsigned)&dmaCb[cbIndex + 1] - (unsigned)virtAddr); dmaCb[cbIndex].nextCbAddress = getAddress(&dmaCb[cbIndex + 1]);
cbIndex++; cbIndex++;
dmaCb[cbIndex].transferInfo = (0x01 << 26) | (0x05 << 16) | (0x01 << 6) | (0x01 << 3); dmaCb[cbIndex].transferInfo = (0x01 << 26) | (0x05 << 16) | (0x01 << 6) | (0x01 << 3);
dmaCb[cbIndex].srcAddress = physAddr + ((unsigned)pwmFifoData - (unsigned)virtAddr); dmaCb[cbIndex].srcAddress = getAddress(pwmFifoData);
dmaCb[cbIndex].dstAddress = PERIPHERALS_PHYS_BASE | (PWM_BASE_OFFSET + 0x18); dmaCb[cbIndex].dstAddress = PERIPHERALS_PHYS_BASE | (PWM_BASE_OFFSET + 0x18);
dmaCb[cbIndex].transferLen = sizeof(unsigned) * PWM_WRITES_PER_SAMPLE; dmaCb[cbIndex].transferLen = sizeof(unsigned) * PWM_WRITES_PER_SAMPLE;
dmaCb[cbIndex].stride = 0; dmaCb[cbIndex].stride = 0;
dmaCb[cbIndex].nextCbAddress = physAddr + ((unsigned)((i < bufferSize - 1) ? &dmaCb[cbIndex + 1] : dmaCb) - (unsigned)virtAddr); dmaCb[cbIndex].nextCbAddress = getAddress((i < bufferSize - 1) ? &dmaCb[cbIndex + 1] : dmaCb);
cbIndex++; cbIndex++;
} }
*pwmFifoData = 0x00; *pwmFifoData = 0x00;
delete samples; delete samples;
DMARegisters *dma = (DMARegisters *)getPeripheral((dmaChannel < 15) ? DMA0_BASE_OFFSET + dmaChannel * 0x100 : DMA15_BASE_OFFSET); volatile DMARegisters *dma = (DMARegisters *)getPeripheral((dmaChannel < 15) ? DMA0_BASE_OFFSET + dmaChannel * 0x100 : DMA15_BASE_OFFSET);
dma->ctlStatus = (0x01 << 31); dma->ctlStatus = (0x01 << 31);
usleep(1000); usleep(1000);
dma->ctlStatus = (0x01 << 2) | (0x01 << 1); dma->ctlStatus = (0x01 << 2) | (0x01 << 1);
dma->cbAddress = physAddr + ((unsigned)dmaCb - (unsigned)virtAddr); dma->cbAddress = getAddress(dmaCb);
dma->ctlStatus = (0xFF << 16) | 0x01; dma->ctlStatus = (0xFF << 16) | 0x01;
usleep(BUFFER_TIME / 2); usleep(BUFFER_TIME >> 2);
bool isError = false;
string errorMessage;
try { try {
while (!eof && transmitting) { while (!eof && transmitting) {
@ -267,7 +294,7 @@ void Transmitter::play(WaveReader &reader, double frequency, unsigned char dmaCh
#ifndef NO_PREEMP #ifndef NO_PREEMP
value = preEmp.filter(value); value = preEmp.filter(value);
#endif #endif
while (i == (((dma->cbAddress - physAddr - ((unsigned)dmaCb - (unsigned)virtAddr)) / sizeof(DMAControllBlock)) >> 1)) { while (i == (((dma->cbAddress - getAddress(dmaCb)) / sizeof(DMAControllBlock)) >> 1)) {
usleep(1); usleep(1);
} }
clkDiv[i] = (0x5A << 24) | (clockDivisor - (int)round(value * 16.0)); clkDiv[i] = (0x5A << 24) | (clockDivisor - (int)round(value * 16.0));
@ -281,7 +308,7 @@ void Transmitter::play(WaveReader &reader, double frequency, unsigned char dmaCh
isError = true; isError = true;
} }
if (eof) { if (eof || isError) {
dmaCb[cbIndex].nextCbAddress = 0x00; dmaCb[cbIndex].nextCbAddress = 0x00;
} else { } else {
dmaCb[(bufferSize - 1) << 1].nextCbAddress = 0x00; dmaCb[(bufferSize - 1) << 1].nextCbAddress = 0x00;
@ -293,21 +320,12 @@ void Transmitter::play(WaveReader &reader, double frequency, unsigned char dmaCh
dma->ctlStatus = (0x01 << 31); dma->ctlStatus = (0x01 << 31);
pwm->ctl = 0x00; pwm->ctl = 0x00;
unmapmem(virtAddr, reqMemSize); freeMemory();
mem_unlock(mbFd, memHandle);
mem_free(mbFd, memHandle);
mbox_close(mbFd);
transmitting = false; transmitting = false;
if (!preserveCarrier) { if (!preserveCarrier) {
clk0->ctl = (0x5A << 24) | 0x06; clk0->ctl = (0x5A << 24) | 0x06;
} }
if (isError) {
throw ErrorReporter(errorMessage);
}
} else { } else {
unsigned sampleOffset = 0; unsigned sampleOffset = 0;
vector<Sample> *buffer = samples; vector<Sample> *buffer = samples;
@ -328,10 +346,7 @@ void Transmitter::play(WaveReader &reader, double frequency, unsigned char dmaCh
throw ErrorReporter(oss.str()); throw ErrorReporter(oss.str());
} }
usleep(BUFFER_TIME / 2); usleep(BUFFER_TIME >> 1);
bool isError = false;
string errorMessage;
try { try {
while (!eof && transmitting) { while (!eof && transmitting) {
@ -346,7 +361,7 @@ void Transmitter::play(WaveReader &reader, double frequency, unsigned char dmaCh
eof = samples->size() < bufferSize; eof = samples->size() < bufferSize;
buffer = samples; buffer = samples;
} }
usleep(BUFFER_TIME / 2); usleep(BUFFER_TIME >> 1);
} }
} }
catch (ErrorReporter &error) { catch (ErrorReporter &error) {
@ -356,14 +371,14 @@ void Transmitter::play(WaveReader &reader, double frequency, unsigned char dmaCh
} }
transmitting = false; transmitting = false;
pthread_join(thread, NULL); pthread_join(thread, NULL);
}
if (isError) { if (isError) {
throw ErrorReporter(errorMessage); throw ErrorReporter(errorMessage);
} }
} }
}
volatile void *Transmitter::getPeripheral(unsigned offset) { void *Transmitter::getPeripheral(unsigned offset) {
return (volatile void *)((unsigned)peripherals + offset); return (void *)((unsigned)peripherals + offset);
} }
void *Transmitter::transmit(void *params) void *Transmitter::transmit(void *params)
@ -382,8 +397,8 @@ void *Transmitter::transmit(void *params)
PreEmp preEmp(*sampleRate); PreEmp preEmp(*sampleRate);
#endif #endif
ClockRegisters *clk0 = (ClockRegisters *)getPeripheral(CLK0_BASE_OFFSET); volatile ClockRegisters *clk0 = (ClockRegisters *)getPeripheral(CLK0_BASE_OFFSET);
unsigned *gpio = (unsigned *)getPeripheral(GPIO_BASE_OFFSET); volatile unsigned *gpio = (unsigned *)getPeripheral(GPIO_BASE_OFFSET);
if (!clockInitialized) { if (!clockInitialized) {
clk0->ctl = (0x5A << 24) | 0x06; clk0->ctl = (0x5A << 24) | 0x06;
usleep(1000); usleep(1000);
@ -393,8 +408,8 @@ void *Transmitter::transmit(void *params)
clockInitialized = true; clockInitialized = true;
} }
TimerRegisters *timer = (TimerRegisters *)getPeripheral(TIMER_BASE_OFFSET); volatile TimerRegisters *timer = (TimerRegisters *)getPeripheral(TIMER_BASE_OFFSET);
unsigned long long current = *(volatile unsigned long long *)&timer->low; unsigned long long current = *(unsigned long long *)&timer->low;
unsigned long long playbackStart = current; unsigned long long playbackStart = current;
while (transmitting) { while (transmitting) {
@ -402,7 +417,7 @@ void *Transmitter::transmit(void *params)
while ((*buffer == NULL) && transmitting) { while ((*buffer == NULL) && transmitting) {
usleep(1); usleep(1);
current = *(volatile unsigned long long *)&timer->low; current = *(unsigned long long *)&timer->low;
} }
if (!transmitting) { if (!transmitting) {
break; break;
@ -427,7 +442,7 @@ void *Transmitter::transmit(void *params)
clk0->div = (0x5A << 24) | (*clockDivisor - (int)round(value * 16.0)); clk0->div = (0x5A << 24) | (*clockDivisor - (int)round(value * 16.0));
while (offset == prevOffset) { while (offset == prevOffset) {
usleep(1); // asm("nop"); usleep(1); // asm("nop");
current = *(volatile unsigned long long *)&timer->low; current = *(unsigned long long *)&timer->low;
offset = (current - start) * (*sampleRate) / 1000000; offset = (current - start) * (*sampleRate) / 1000000;
} }
} }

8
transmitter.h
Wyświetl plik

@ -49,11 +49,17 @@ class Transmitter
Transmitter(); Transmitter();
Transmitter(const Transmitter &source); Transmitter(const Transmitter &source);
Transmitter &operator=(const Transmitter &source); Transmitter &operator=(const Transmitter &source);
static volatile void *getPeripheral(unsigned offset); bool allocateMemory(unsigned size);
void freeMemory();
unsigned getAddress(volatile void *object);
static void *getPeripheral(unsigned offset);
static void *transmit(void *params); static void *transmit(void *params);
static void *peripherals; static void *peripherals;
static bool transmitting, clockInitialized, preserveCarrier; static bool transmitting, clockInitialized, preserveCarrier;
unsigned memSize, memAddress, memHandle;
void *memAllocated;
int mBoxFd;
}; };
#endif // TRANSMITTER_H #endif // TRANSMITTER_H