From 5d308e7cd07e8dd39f4e5e8be452f856056e6767 Mon Sep 17 00:00:00 2001
From: Marcin Kondej <markondej@gmail.com>
Date: Thu, 12 Sep 2019 15:57:38 +0200
Subject: [PATCH] Different PLLD frequency support

---
 transmitter.cpp | 20 ++++++++++++++------
 transmitter.h   |  1 +
 2 files changed, 15 insertions(+), 6 deletions(-)

diff --git a/transmitter.cpp b/transmitter.cpp
index 09bb68e..7acfdf4 100644
--- a/transmitter.cpp
+++ b/transmitter.cpp
@@ -55,13 +55,16 @@ using std::vector;
 #define PWM_BASE_OFFSET 0x0020C000
 #define TIMER_BASE_OFFSET 0x00003000
 
-#define BCM2837_MEM_FLAG 0x04
 #define BCM2835_MEM_FLAG 0x0C
-#define PAGE_SIZE 4096
+#define BCM2838_MEM_FLAG 0x04
+
+#define BCM2835_PLLD_FREQ 500
+#define BCM2838_PLLD_FREQ 750
 
 #define BUFFER_TIME 1000000
 #define PWM_WRITES_PER_SAMPLE 10
 #define PWM_CHANNEL_RANGE 32
+#define PAGE_SIZE 4096
 
 struct TimerRegisters {
     unsigned ctlStatus;
@@ -155,7 +158,7 @@ bool Transmitter::allocateMemory(unsigned size)
     if (memSize % PAGE_SIZE) {
         memSize = (memSize / PAGE_SIZE + 1) * PAGE_SIZE;
     }
-    memHandle = mem_alloc(mBoxFd, size, PAGE_SIZE, (getPeripheralVirtAddress() == BCM2835_PERI_VIRT_BASE) ? BCM2835_MEM_FLAG : BCM2837_MEM_FLAG);
+    memHandle = mem_alloc(mBoxFd, size, PAGE_SIZE, (getPeripheralVirtAddress() == BCM2835_PERI_VIRT_BASE) ? BCM2835_MEM_FLAG : BCM2838_MEM_FLAG);
     if (!memHandle) {
         mbox_close(mBoxFd);
         memSize = 0;
@@ -190,6 +193,11 @@ unsigned Transmitter::getPeripheralSize()
     return size;
 }
 
+float Transmitter::getSrcClkFreq()
+{
+    return (getPeripheralVirtAddress() == BCM2838_PERI_VIRT_BASE) ? BCM2838_PLLD_FREQ : BCM2835_PLLD_FREQ;
+}
+
 unsigned Transmitter::getMemoryAddress(volatile void *object)
 {
     return (memSize) ? memAddress + ((unsigned)object - (unsigned)memAllocated) : 0x00000000;
@@ -220,8 +228,8 @@ void Transmitter::play(WaveReader &reader, double frequency, double bandwidth, u
     }
     bool eof = samples->size() < bufferSize;
 
-    unsigned clockDivisor = (unsigned)round((500 << 12) / frequency);
-    unsigned divisorRange = clockDivisor - (unsigned)round((500 << 12) / (frequency + 0.0005 * bandwidth));
+    unsigned clockDivisor = (unsigned)round(getSrcClkFreq() * (0x01 << 12) / frequency);
+    unsigned divisorRange = clockDivisor - (unsigned)round(getSrcClkFreq() * (0x01 << 12) / (frequency + 0.0005 * bandwidth));
     bool isError = false;
     string errorMessage;
 
@@ -251,7 +259,7 @@ void Transmitter::play(WaveReader &reader, double frequency, double bandwidth, u
         float pwmClkFreq = PWM_WRITES_PER_SAMPLE * PWM_CHANNEL_RANGE * header.sampleRate / 1000000;
         pwmClk->ctl = (0x5A << 24) | 0x06;
         usleep(1000);
-        pwmClk->div = (0x5A << 24) | (unsigned)((500 << 12) / pwmClkFreq);
+        pwmClk->div = (0x5A << 24) | (unsigned)(getSrcClkFreq() * (0x01 << 12) / pwmClkFreq);
         pwmClk->ctl = (0x5A << 24) | (0x01 << 4) | 0x06;
 
         volatile PWMRegisters *pwm = (PWMRegisters *)getPeripheral(PWM_BASE_OFFSET);
diff --git a/transmitter.h b/transmitter.h
index 5a0662f..de2a626 100644
--- a/transmitter.h
+++ b/transmitter.h
@@ -51,6 +51,7 @@ class Transmitter
         Transmitter &operator=(const Transmitter &source);
         bool allocateMemory(unsigned size);
         void freeMemory();
+        float getSrcClkFreq();
         unsigned getPeripheralVirtAddress();
         unsigned getPeripheralSize();		
         unsigned getMemoryAddress(volatile void *object);