From 28651d5e3a9e33b29448948cee6ff10abf6284d0 Mon Sep 17 00:00:00 2001
From: F5OEO <evaristec@gmail.com>
Date: Tue, 27 Feb 2018 09:31:46 +0000
Subject: [PATCH] Add pwm

---
 src/gpio.cpp | 82 +++++++++++++++++++++++++++++++++++++++++++++++++++-
 src/gpio.h   | 54 +++++++++++++++++++++++++++++++++-
 2 files changed, 134 insertions(+), 2 deletions(-)

diff --git a/src/gpio.cpp b/src/gpio.cpp
index eeb7652..de7ba90 100644
--- a/src/gpio.cpp
+++ b/src/gpio.cpp
@@ -5,6 +5,7 @@ extern "C"
 #include "gpio.h"
 #include "raspberry_pi_revision.h"
 #include "stdio.h"
+#include <unistd.h>
 
 gpio::gpio(uint32_t base, uint32_t len)
 {
@@ -56,6 +57,11 @@ clkgpio::clkgpio():gpio(GetPeripheralBase()+CLK_BASE,CLK_LEN)
 {
 }
 
+clkgpio::~clkgpio()
+{
+	gpioreg[GPCLK_CNTL]= 0x5A000000 | (Mash << 9) | pllnumber|(0 << 4)  ; //4 is START CLK
+}
+
 int clkgpio::SetPllNumber(int PllNo,int MashType)
 {
 	if(PllNo<8)
@@ -94,7 +100,7 @@ int clkgpio::SetFrequency(uint64_t Frequency)
 	uint32_t FreqDivider=(uint32_t)Freqresult;
 	uint32_t FreqFractionnal=(uint32_t) (4096*(Freqresult-(double)FreqDivider));
 
-	printf("DIV/FRAC %u/%u \n",FreqDivider,FreqFractionnal);
+	//printf("DIV/FRAC %u/%u \n",FreqDivider,FreqFractionnal);
 
 	gpioreg[GPCLK_DIV] = 0x5A000000 | ((FreqDivider)<<12) | FreqFractionnal;
 	//gpioreg[GPCLK_CNTL]= 0x5A000000 | (Mash << 9) | pllnumber |4  ; //4 is START CLK
@@ -195,13 +201,87 @@ void clkgpio::print_clock_tree(void)
 
 
 // ************************************** GENERAL GPIO *****************************************************
+
 generalgpio::generalgpio():gpio(GetPeripheralBase()+GENERAL_BASE,GENERAL_LEN)
 {
 }
 
+generalgpio::~generalgpio()
+{
+	disableclk();
+}
+
 void generalgpio::enableclk()
 {
 	gpioreg[GPFSEL0] = (gpioreg[GPFSEL0] & ~(7 << 12)) | (4 << 12);
 }
 
+void generalgpio::disableclk()
+{
+	gpioreg[GPFSEL0] = (gpioreg[GPFSEL0] & ~(7 << 12)) | (0 << 12);
+}
+
+// ********************************** PWM GPIO **********************************
+
+pwmgpio::pwmgpio():gpio(GetPeripheralBase()+PWM_BASE,PWM_LEN)
+{
+	gpioreg[PWM_CTL] = 0;
+}
+
+pwmgpio::~pwmgpio()
+{
+	gpioreg[PWM_CTL] = 0;
+}
+
+int pwmgpio::SetPllNumber(int PllNo,int MashType)
+{
+	if(PllNo<8)
+		pllnumber=PllNo;
+	else
+		pllnumber=clk_pllc;	
+	if(MashType<4)
+		Mash=MashType;
+	else
+		Mash=0;
+	clk.gpioreg[PWMCLK_CNTL]= 0x5A000000 | (Mash << 9) | pllnumber|(1 << 4)  ; //4 is START CLK
+	Pllfrequency=GetPllFrequency(pllnumber);
+	return 0;
+}
+
+uint64_t pwmgpio::GetPllFrequency(int PllNo)
+{
+	return clk.GetPllFrequency(PllNo);
+
+}
+
+int pwmgpio::SetFrequency(uint64_t Frequency)
+{
+	
+	double Freqresult=(double)Pllfrequency/(double)Frequency;
+	uint32_t FreqDivider=(uint32_t)Freqresult;
+	uint32_t FreqFractionnal=(uint32_t) (4096*(Freqresult-(double)FreqDivider));
+	clk.gpioreg[PWMCLK_DIV] = 0x5A000000 | ((FreqDivider)<<12) | FreqFractionnal;
+	
+	return 0;
+
+}
+
+int pwmgpio::SetMode(int Mode)
+{
+		gpioreg[PWM_RNG1] = 32;// 250 -> 8KHZ
+		usleep(100);
+		gpioreg[PWM_RNG2] = 32;// 32 Mandatory for Serial Mode without gap
+	
+		gpioreg[PWM_FIFO]=0xAAAAAAAA;
+		gpioreg[PWM_DMAC] = PWMDMAC_ENAB | PWMDMAC_THRSHLD;
+		usleep(100);
+		gpioreg[PWM_CTL] = PWMCTL_CLRF;
+		
+		//pwm_reg[PWM_CTL] =   PWMCTL_USEF1| PWMCTL_MODE1| PWMCTL_PWEN1|PWMCTL_RPTL1; //PWM0 in Repeat mode
+	
+	
+	return 0;
+
+}
+		
 
diff --git a/src/gpio.h b/src/gpio.h
index b0f41db..778d866 100644
--- a/src/gpio.h
+++ b/src/gpio.h
@@ -65,6 +65,7 @@ class dmagpio:public gpio
 #define EMMCCLK_CNTL     (0x1C0/4)
 #define EMMCCLK_DIV      (0x1C4/4)
 
+
 #define PLLA_CTRL (0x1100/4)
 #define PLLA_FRAC (0x1200/4)
 #define PLLA_DSI0 (0x1300/4)
@@ -113,6 +114,7 @@ class clkgpio:public gpio
 	
     public:
     clkgpio();
+	~clkgpio();
 	int SetPllNumber(int PllNo,int MashType);
 	uint64_t GetPllFrequency(int PllNo);
 	void print_clock_tree(void);
@@ -138,7 +140,57 @@ class generalgpio:public gpio
     
     public:
     generalgpio();
-    void enableclk();    
+	~generalgpio();
+    void enableclk();  
+	void disableclk();
 };
 
+//************************************ PWM GPIO ***************************************
+
+#define PWM_BASE		(0x0020C000)
+#define PWM_LEN			0x28
+
+#define PWM_CTL			(0x00/4)
+#define PWM_DMAC		(0x08/4)
+#define PWM_RNG1		(0x10/4)
+#define PWM_RNG2		(0x20/4)
+#define PWM_FIFO		(0x18/4)
+
+#define PWMCLK_CNTL		(0x100/4) // Clk register
+#define PWMCLK_DIV		(0x104/4) // Clk register
+
+
+#define PWMCTL_MSEN2 (1<<15)
+#define PWMCTL_USEF2 (1<<13)
+#define PWMCTL_RPTL2 (1<<10)
+#define PWMCTL_MODE2 (1<<9)
+#define PWMCTL_PWEN2 (1<<8)
+
+#define PWMCTL_MSEN1 (1<<7)
+#define PWMCTL_CLRF (1<<6)
+#define PWMCTL_USEF1 (1<<5)
+#define PWMCTL_POLA1 (1<<4)
+#define PWMCTL_RPTL1 (1<<2)
+#define PWMCTL_MODE1 (1<<1)
+#define PWMCTL_PWEN1 (1<<0)
+#define PWMDMAC_ENAB		(1<<31)
+#define PWMDMAC_THRSHLD		((15<<8)|(15<<0))
+
+class pwmgpio:public gpio
+{
+    protected:
+	clkgpio clk;
+	int pllnumber;
+	int Mash;
+	uint64_t Pllfrequency;
+    public:
+    pwmgpio();
+	~pwmgpio();
+	int SetPllNumber(int PllNo,int MashType);
+	uint64_t GetPllFrequency(int PllNo);
+	int SetFrequency(uint64_t Frequency);
+   int SetMode(int Mode);
+};
+
+
 #endif