PROGRAM CODE :
#include "transmitter.hpp"
#include "mailbox.hpp"
#include <bcm_host.h>
#include <thread>
#include <chrono>
#include <cmath>
#include <fcntl.h>
#include <sys/mman.h>
#define PERIPHERALS_PHYS_BASE 0x7e000000
#define BCM2835_PERI_VIRT_BASE 0x20000000
#define BCM2711_PERI_VIRT_BASE 0xfe000000
#define BCM2835_MEM_FLAG 0x0c
#define BCM2711_MEM_FLAG 0x04
#define BCM2835_PLLD_FREQ 500
#define BCM2711_PLLD_FREQ 750
#define GPIO_BASE_OFFSET 0x00200000
#define CLK0_BASE_OFFSET 0x00101070
#define CLK1_BASE_OFFSET 0x00101078
#define CLK_PASSWORD (0x5a << 24)
#define CLK_CTL_SRC_PLLA 0x04
#define CLK_CTL_SRC_PLLC 0x05
#define CLK_CTL_SRC_PLLD 0x06
#define CLK_CTL_ENAB (0x01 << 4)
#define CLK_CTL_MASH(x) ((x & 0x03) << 9)
#define PWMCLK_BASE_OFFSET 0x001010a0
#define PWM_BASE_OFFSET 0x0020c000
#define PWM_CHANNEL_RANGE 32
#define PWM_WRITES_PER_SAMPLE 10
#define PWM_CTL_CLRF1 (0x01 << 6)
#define PWM_CTL_USEF1 (0x01 << 5)
#define PWM_CTL_RPTL1 (0x01 << 2)
#define PWM_CTL_MODE1 (0x01 << 1)
#define PWM_CTL_PWEN1 0x01
#define PWM_STA_BERR (0x01 << 8)
#define PWM_STA_GAPO4 (0x01 << 7)
#define PWM_STA_GAPO3 (0x01 << 6)
#define PWM_STA_GAPO2 (0x01 << 5)
#define PWM_STA_GAPO1 (0x01 << 4)
#define PWM_STA_RERR1 (0x01 << 3)
#define PWM_STA_WERR1 (0x01 << 2)
#define PWM_STA_EMPT1 (0x01 << 1)
#define PWM_STA_FULL1 0x01
#define PWM_DMAC_ENAB (0x01 << 31)
#define PWM_DMAC_PANIC(x) ((x & 0x0f) << 8)
#define PWM_DMAC_DREQ(x) (x & 0x0f)
#define DMA0_BASE_OFFSET 0x00007000
#define DMA15_BASE_OFFSET 0x00e05000
#define DMA_CS_RESET (0x01 << 31)
#define DMA_CS_PANIC_PRIORITY(x) ((x & 0x0f) << 20)
#define DMA_CS_PRIORITY(x) ((x & 0x0f) << 16)
#define DMA_CS_INT (0x01 << 2)
#define DMA_CS_END (0x01 << 1)
#define DMA_CS_ACTIVE 0x01
#define DMA_TI_NO_WIDE_BURST (0x01 << 26)
#define DMA_TI_PERMAP(x) ((x & 0x0f) << 16)
#define DMA_TI_DEST_DREQ (0x01 << 6)
#define DMA_TI_WAIT_RESP (0x01 << 3)
#define BUFFER_TIME 1000000
#define PAGE_SIZE 4096
struct ClockRegisters {
uint32_t ctl;
uint32_t div;
};
struct PWMRegisters {
uint32_t ctl;
uint32_t status;
uint32_t dmaConf;
uint32_t reserved0;
uint32_t chn1Range;
uint32_t chn1Data;
uint32_t fifoIn;
uint32_t reserved1;
uint32_t chn2Range;
uint32_t chn2Data;
};
struct DMAControllBlock {
uint32_t transferInfo;
uint32_t srcAddress;
uint32_t dstAddress;
uint32_t transferLen;
uint32_t stride;
uint32_t nextCbAddress;
uint32_t reserved0;
uint32_t reserved1;
};
struct DMARegisters {
uint32_t ctlStatus;
uint32_t cbAddress;
uint32_t transferInfo;
uint32_t srcAddress;
uint32_t dstAddress;
uint32_t transferLen;
uint32_t stride;
uint32_t nextCbAddress;
uint32_t debug;
};
class Peripherals
public:
virtual ~Peripherals() {
munmap(peripherals, GetSize());
Peripherals(const Peripherals &) = delete;
Peripherals(Peripherals &&) = delete;
Peripherals &operator=(const Peripherals &) = delete;
static Peripherals &GetInstance() {
static Peripherals instance;
return instance;
uintptr_t GetPhysicalAddress(volatile void *object) const {
return PERIPHERALS_PHYS_BASE + (reinterpret_cast<uintptr_t>(object) -
reinterpret_cast<uintptr_t>(peripherals));
uintptr_t GetVirtualAddress(uintptr_t offset) const {
return reinterpret_cast<uintptr_t>(peripherals) + offset;
static uintptr_t GetVirtualBaseAddress() {
return (bcm_host_get_peripheral_size() == BCM2711_PERI_VIRT_BASE) ?
BCM2711_PERI_VIRT_BASE : bcm_host_get_peripheral_address();
static float GetClockFrequency() {
return (Peripherals::GetVirtualBaseAddress() == BCM2711_PERI_VIRT_BASE) ?
BCM2711_PLLD_FREQ : BCM2835_PLLD_FREQ;
private:
Peripherals() {
int memFd;
if ((memFd = open("/dev/mem", O_RDWR | O_SYNC)) < 0) {
throw std::runtime_error("Cannot open /dev/mem file (permission denied)");
peripherals = mmap(nullptr, GetSize(), PROT_READ | PROT_WRITE, MAP_SHARED, memFd,
GetVirtualBaseAddress());
close(memFd);
if (peripherals == MAP_FAILED) {
throw std::runtime_error("Cannot obtain access to peripherals (mmap error)");
}
}
unsigned GetSize() {
unsigned size = bcm_host_get_peripheral_size();
if (size == BCM2711_PERI_VIRT_BASE) {
size = 0x01000000;
return size;
void *peripherals;
};
class AllocatedMemory
public:
AllocatedMemory() = delete;
AllocatedMemory(unsigned size) {
mBoxFd = mbox_open();
memSize = size;
if (memSize % PAGE_SIZE) {
memSize = (memSize / PAGE_SIZE + 1) * PAGE_SIZE;
memHandle = mem_alloc(mBoxFd, size, PAGE_SIZE, (Peripherals::GetVirtualBaseAddress() ==
BCM2835_PERI_VIRT_BASE) ? BCM2835_MEM_FLAG : BCM2711_MEM_FLAG);
if (!memHandle) {
mbox_close(mBoxFd);
memSize = 0;
throw std::runtime_error("Cannot allocate memory (" + std::to_string(size) + " bytes)");
memAddress = mem_lock(mBoxFd, memHandle);
memAllocated = mapmem(memAddress & ~0xc0000000, memSize);
}
virtual ~AllocatedMemory() {
unmapmem(memAllocated, memSize);
mem_unlock(mBoxFd, memHandle);
mem_free(mBoxFd, memHandle);
mbox_close(mBoxFd);
memSize = 0;
AllocatedMemory(const AllocatedMemory &) = delete;
AllocatedMemory(AllocatedMemory &&) = delete;
AllocatedMemory &operator=(const AllocatedMemory &) = delete;
uintptr_t GetPhysicalAddress(volatile void *object) const {
return (memSize) ? memAddress + (reinterpret_cast<uintptr_t>(object) -
reinterpret_cast<uintptr_t>(memAllocated)) : 0x00000000;
uintptr_t GetBaseAddress() const {
return reinterpret_cast<uintptr_t>(memAllocated);
private:
unsigned memSize, memHandle;
uintptr_t memAddress;
void *memAllocated;
int mBoxFd;
};
class Device
public:
Device() {
peripherals = &Peripherals::GetInstance();
}
Device(const Device &) = delete;
Device(Device &&) = delete;
Device &operator=(const Device &) = delete;
protected:
Peripherals *peripherals;
};
class ClockDevice : public Device
public:
ClockDevice() = delete;
ClockDevice(uintptr_t address, unsigned divisor) {
clock = reinterpret_cast<ClockRegisters *>(peripherals->GetVirtualAddress(address));
clock->ctl = CLK_PASSWORD | CLK_CTL_SRC_PLLD;
std::this_thread::sleep_for(std::chrono::microseconds(100));
clock->div = CLK_PASSWORD | (0xffffff & divisor);
clock->ctl = CLK_PASSWORD | CLK_CTL_MASH(0x1) | CLK_CTL_ENAB |
CLK_CTL_SRC_PLLD; }
virtual ~ClockDevice() {
clock->ctl = CLK_PASSWORD | CLK_CTL_SRC_PLLD;
protected:
volatile ClockRegisters *clock;
};
class ClockOutput : public ClockDevice
public:
ClockOutput() = delete;
#ifndef GPIO21
ClockOutput(unsigned divisor) : ClockDevice(CLK0_BASE_OFFSET, divisor) {
output = reinterpret_cast<uint32_t *>(peripherals->GetVirtualAddress(GPIO_BASE_OFFSET));
*output = (*output & 0xffff8fff) | (0x04 << 12);
#else
ClockOutput(unsigned divisor) : ClockDevice(CLK1_BASE_OFFSET, divisor) {
output = reinterpret_cast<uint32_t *>(peripherals->GetVirtualAddress(GPIO_BASE_OFFSET +
0x08));
*output = (*output & 0xffffffc7) | (0x02 << 3);
#endif
virtual ~ClockOutput() {
#ifndef GPIO21
*output = (*output & 0xffff8fff) | (0x01 << 12);
#else
*output = (*output & 0xffffffc7) | (0x02 << 3);
#endif
void SetDivisor(unsigned divisor) {
clock->div = CLK_PASSWORD | (0xffffff & divisor);
volatile uint32_t &GetDivisor() {
return clock->div;
private:
volatile uint32_t *output;
};
class PWMController : public ClockDevice
public:
PWMController() = delete;
PWMController(unsigned sampleRate) : ClockDevice(PWMCLK_BASE_OFFSET,
static_cast<unsigned>(Peripherals::GetClockFrequency() * 1000000.f * (0x01 << 12) /
(PWM_WRITES_PER_SAMPLE * PWM_CHANNEL_RANGE * sampleRate))) {
pwm = reinterpret_cast<PWMRegisters *>(peripherals-
>GetVirtualAddress(PWM_BASE_OFFSET));
pwm->ctl = 0x00000000;
std::this_thread::sleep_for(std::chrono::microseconds(100));
pwm->status = PWM_STA_BERR | PWM_STA_GAPO1 | PWM_STA_RERR1 |
PWM_STA_WERR1;
pwm->ctl = PWM_CTL_CLRF1;
std::this_thread::sleep_for(std::chrono::microseconds(100));
pwm->chn1Range = PWM_CHANNEL_RANGE;
pwm->dmaConf = PWM_DMAC_ENAB | PWM_DMAC_PANIC(0x7) |
PWM_DMAC_DREQ(0x7);
pwm->ctl = PWM_CTL_USEF1 | PWM_CTL_RPTL1 | PWM_CTL_MODE1 |
PWM_CTL_PWEN1;
virtual ~PWMController() {
pwm->ctl = 0x00000000;
volatile uint32_t &GetFifoIn() {
return pwm->fifoIn;
private:
volatile PWMRegisters *pwm;
};
class DMAController : public Device
public:
DMAController() = delete;
DMAController(uint32_t address, unsigned dmaChannel) {
dma = reinterpret_cast<DMARegisters *>(peripherals->GetVirtualAddress((dmaChannel < 15) ?
DMA0_BASE_OFFSET + dmaChannel * 0x100 : DMA15_BASE_OFFSET));
dma->ctlStatus = DMA_CS_RESET;
std::this_thread::sleep_for(std::chrono::microseconds(100));
dma->ctlStatus = DMA_CS_INT | DMA_CS_END;
dma->cbAddress = address;
dma->ctlStatus = DMA_CS_PANIC_PRIORITY(0xf) | DMA_CS_PRIORITY(0xf) |
DMA_CS_ACTIVE;
virtual ~DMAController() {
dma->ctlStatus = DMA_CS_RESET;
void SetControllBlockAddress(uint32_t address) {
dma->cbAddress = address;
volatile uint32_t &GetControllBlockAddress() {
return dma->cbAddress;
private:
volatile DMARegisters *dma;
};
Transmitter::Transmitter()
: output(nullptr), enable(false)
Transmitter::~Transmitter() {
std::unique_lock<std::mutex> lock(mtx);
cv.wait(lock, [&]() -> bool {
return !enable;
});
if (output) {
delete output;
void Transmitter::Transmit(WaveReader &reader, float frequency, float bandwidth, unsigned dmaChannel,
bool preserveCarrier)
std::lock_guard<std::mutex> lock(mtx);
enable = true;
auto finally = [&]() {
if (!preserveCarrier && output) {
delete output;
output = nullptr;
std::lock_guard<std::mutex> lock(mtx);
enable = false;
cv.notify_all();
};
try {
WaveHeader header = reader.GetHeader();
unsigned bufferSize = static_cast<unsigned>(static_cast<unsigned long long>(header.sampleRate) *
BUFFER_TIME / 1000000);
unsigned clockDivisor = static_cast<unsigned>(round(Peripherals::GetClockFrequency() * (0x01 <<
12) / frequency));
unsigned divisorRange = clockDivisor -
static_cast<unsigned>(round(Peripherals::GetClockFrequency() * (0x01 << 12) / (frequency + 0.0005f *
bandwidth)));
if (!output) {
output = new ClockOutput(clockDivisor);
if (dmaChannel != 0xff) {
TxViaDma(reader, header.sampleRate, bufferSize, clockDivisor, divisorRange, dmaChannel);
} else {
TxViaCpu(reader, header.sampleRate, bufferSize, clockDivisor, divisorRange);
} catch (...) {
finally();
throw;
finally();
void Transmitter::Stop()
std::unique_lock<std::mutex> lock(mtx);
enable = false;
lock.unlock();
cv.notify_all();
void Transmitter::TxViaDma(WaveReader &reader, unsigned sampleRate, unsigned bufferSize, unsigned
clockDivisor, unsigned divisorRange, unsigned dmaChannel)
{
if (dmaChannel > 15) {
throw std::runtime_error("DMA channel number out of range (0 - 15)");
AllocatedMemory allocated(sizeof(uint32_t) * bufferSize + sizeof(DMAControllBlock) * (2 * bufferSize)
+ sizeof(uint32_t));
std::vector<Sample> samples = reader.GetSamples(bufferSize, enable, mtx);
if (samples.empty()) {
return;
bool eof = false;
if (samples.size() < bufferSize) {
bufferSize = samples.size();
eof = true;
PWMController pwm(sampleRate);
Peripherals &peripherals = Peripherals::GetInstance();
unsigned cbOffset = 0;
volatile DMAControllBlock *dmaCb = reinterpret_cast<DMAControllBlock
*>(allocated.GetBaseAddress());
volatile uint32_t *clkDiv = reinterpret_cast<uint32_t *>(reinterpret_cast<uintptr_t>(dmaCb) + 2 *
sizeof(DMAControllBlock) * bufferSize);
volatile uint32_t *pwmFifoData = reinterpret_cast<uint32_t *>(reinterpret_cast<uintptr_t>(clkDiv) +
sizeof(uint32_t) * bufferSize);
for (unsigned i = 0; i < bufferSize; i++) {
float value = samples[i].GetMonoValue();
clkDiv[i] = CLK_PASSWORD | (0xffffff & (clockDivisor - static_cast<int32_t>(round(value *
divisorRange))));
dmaCb[cbOffset].transferInfo = DMA_TI_NO_WIDE_BURST | DMA_TI_WAIT_RESP;;
dmaCb[cbOffset].srcAddress = allocated.GetPhysicalAddress(&clkDiv[i]);
dmaCb[cbOffset].dstAddress = peripherals.GetPhysicalAddress(&output->GetDivisor());
dmaCb[cbOffset].transferLen = sizeof(uint32_t);
dmaCb[cbOffset].stride = 0;
dmaCb[cbOffset].nextCbAddress = allocated.GetPhysicalAddress(&dmaCb[cbOffset + 1]);
cbOffset++;
dmaCb[cbOffset].transferInfo = DMA_TI_NO_WIDE_BURST | DMA_TI_PERMAP(0x5) |
DMA_TI_DEST_DREQ | DMA_TI_WAIT_RESP;
dmaCb[cbOffset].srcAddress = allocated.GetPhysicalAddress(pwmFifoData);
dmaCb[cbOffset].dstAddress = peripherals.GetPhysicalAddress(&pwm.GetFifoIn());
dmaCb[cbOffset].transferLen = sizeof(uint32_t) * PWM_WRITES_PER_SAMPLE;
dmaCb[cbOffset].stride = 0;
dmaCb[cbOffset].nextCbAddress = allocated.GetPhysicalAddress((i < bufferSize - 1) ?
&dmaCb[cbOffset + 1] : dmaCb);
cbOffset++;
*pwmFifoData = 0x00000000;
DMAController dma(allocated.GetPhysicalAddress(dmaCb), dmaChannel);
std::this_thread::sleep_for(std::chrono::microseconds(BUFFER_TIME / 10));
auto finally = [&]() {
dmaCb[(cbOffset < 2 * bufferSize) ? cbOffset : 0].nextCbAddress = 0x00000000;
while (dma.GetControllBlockAddress() != 0x00000000) {
std::this_thread::sleep_for(std::chrono::milliseconds(1));
}
samples.clear();
};
try {
while (!eof) {
std::lock_guard<std::mutex> lock(mtx);
if (!enable) {
break;
samples = reader.GetSamples(bufferSize, enable, mtx);
if (!samples.size()) {
break;
cbOffset = 0;
eof = samples.size() < bufferSize;
for (std::size_t i = 0; i < samples.size(); i++) {
float value = samples[i].GetMonoValue();
while (i == ((dma.GetControllBlockAddress() - allocated.GetPhysicalAddress(dmaCb)) / (2 *
sizeof(DMAControllBlock)))) {
std::this_thread::sleep_for(std::chrono::microseconds(BUFFER_TIME / 10));
clkDiv[i] = CLK_PASSWORD | (0xffffff & (clockDivisor - static_cast<int>(round(value *
divisorRange))));
cbOffset += 2;
} catch (...) {
finally();
throw;
}
finally();
void Transmitter::TxViaCpu(WaveReader &reader, unsigned sampleRate, unsigned bufferSize, unsigned
clockDivisor, unsigned divisorRange)
std::vector<Sample> samples;
unsigned sampleOffset = 0;
bool eof = false, stop = false, start = true;
txThread = std::thread(&Transmitter::CpuTxThread, this, sampleRate, clockDivisor, divisorRange,
&sampleOffset, &samples, &stop);
auto finally = [&]() {
std::lock_guard<std::mutex> lock(mtx);
stop = true;
cv.notify_all();
txThread.join();
samples.clear();
};
try {
while (!eof) {
std::unique_lock<std::mutex> lock(mtx);
if (!start) {
cv.wait(lock, [&]() -> bool {
return samples.empty() || !enable || stop;
});
}
if (!enable) {
break;
if (stop) {
throw std::runtime_error("Transmitter thread has unexpectedly exited");
if (samples.empty()) {
if (!reader.SetSampleOffset(sampleOffset + (start ? 0 : bufferSize))) {
break;
lock.unlock();
samples = reader.GetSamples(bufferSize, enable, mtx);
lock.lock();
if (samples.empty()) {
break;
eof = samples.size() < bufferSize;
lock.unlock();
cv.notify_all();
} else {
lock.unlock();
start = false;
} catch (...) {
finally();
throw;
finally();
}
void Transmitter::CpuTxThread(unsigned sampleRate, unsigned clockDivisor, unsigned divisorRange,
unsigned *sampleOffset, std::vector<Sample> *samples, bool *stop)
try {
auto playbackStart = std::chrono::system_clock::now();
std::chrono::system_clock::time_point current, start;
while (true) {
std::vector<Sample> loadedSamples;
std::unique_lock<std::mutex> lock(mtx);
cv.wait(lock, [&]() -> bool {
return !samples->empty() || *stop;
});
if (*stop) {
break;
start = current = std::chrono::system_clock::now();
*sampleOffset = std::chrono::duration_cast<std::chrono::microseconds>(current -
playbackStart).count() * sampleRate / 1000000;
loadedSamples = std::move(*samples);
lock.unlock();
cv.notify_all();
unsigned offset = 0;
while (true) {
if (offset >= loadedSamples.size()) {
break;
}
unsigned prevOffset = offset;
float value = loadedSamples[offset].GetMonoValue();
output->SetDivisor(clockDivisor - static_cast<int>(round(value * divisorRange)));
while (offset == prevOffset) {
std::this_thread::yield(); // asm("nop");
current = std::chrono::system_clock::now();
offset = std::chrono::duration_cast<std::chrono::microseconds>(current - start).count() *
sampleRate / 1000000;
} catch (...) {
std::unique_lock<std::mutex> lock(mtx);
*stop = true;
lock.unlock();
cv.notify_all();
