Skip to content

Commit 916bb82

Browse files
author
Craig Altrenburg
committed
Added PWM changes from kaiede
1 parent 9924a33 commit 916bb82

1 file changed

Lines changed: 160 additions & 76 deletions

File tree

‎Sources/PWM.swift‎

Lines changed: 160 additions & 76 deletions
Original file line numberDiff line numberDiff line change
@@ -50,19 +50,25 @@ extension SwiftyGPIO {
5050

5151
// RaspberryPis ARMv6 (all 1, Zero, Zero W) PWMs, only accessible ones, divided in channels (can use only one for each channel)
5252
static let PWMRPI1: [Int:[GPIOName:PWMOutput]] = [
53-
0: [.P12: RaspberryPWM(gpioId: 12, alt: 0, channel:0, baseAddr: 0x20000000), .P18: RaspberryPWM(gpioId: 18, alt: 5, channel:0, baseAddr: 0x20000000)],
54-
1: [.P13: RaspberryPWM(gpioId: 13, alt: 0, channel:1, baseAddr: 0x20000000), .P19: RaspberryPWM(gpioId: 19, alt: 5, channel:1, baseAddr: 0x20000000)]
53+
0: [.P12: RaspberryPWM(gpioId: 12, alt: 0, channel:0, phy: RaspberryPHY.phyFor(baseAddr: 0x20000000)),
54+
.P18: RaspberryPWM(gpioId: 18, alt: 5, channel:0, phy: RaspberryPHY.phyFor(baseAddr: 0x20000000))],
55+
1: [.P13: RaspberryPWM(gpioId: 13, alt: 0, channel:1, phy: RaspberryPHY.phyFor(baseAddr: 0x20000000)),
56+
.P19: RaspberryPWM(gpioId: 19, alt: 5, channel:1, phy: RaspberryPHY.phyFor(baseAddr: 0x20000000))]
5557
]
5658

5759
// RaspberryPis ARMv7 (2-3) PWMs, only accessible ones, divided in channels (can use only one for each channel)
5860
static let PWMRPI23: [Int:[GPIOName:PWMOutput]] = [
59-
0: [.P12: RaspberryPWM(gpioId: 12, alt: 0, channel:0, baseAddr: 0x3F000000), .P18: RaspberryPWM(gpioId: 18, alt: 5, channel:0, baseAddr: 0x3F000000)],
60-
1: [.P13: RaspberryPWM(gpioId: 13, alt: 0, channel:1, baseAddr: 0x3F000000), .P19: RaspberryPWM(gpioId: 19, alt: 5, channel:1, baseAddr: 0x3F000000)]
61+
0: [.P12: RaspberryPWM(gpioId: 12, alt: 0, channel:0, phy: RaspberryPHY.phyFor(baseAddr: 0x3F000000)),
62+
.P18: RaspberryPWM(gpioId: 18, alt: 5, channel:0, phy: RaspberryPHY.phyFor(baseAddr: 0x3F000000))],
63+
1: [.P13: RaspberryPWM(gpioId: 13, alt: 0, channel:1, phy: RaspberryPHY.phyFor(baseAddr: 0x3F000000)),
64+
.P19: RaspberryPWM(gpioId: 19, alt: 5, channel:1, phy: RaspberryPHY.phyFor(baseAddr: 0x3F000000))]
6165
]
6266
// RaspberryPi 4 PWMs, only accessible ones, divided in channels (can use only one for each channel)
6367
static let PWMRPI4: [Int:[GPIOName:PWMOutput]] = [
64-
0: [.P12: RaspberryPWM(gpioId: 12, alt: 0, channel:0, baseAddr: 0xFE000000), .P18: RaspberryPWM(gpioId: 18, alt: 5, channel:0, baseAddr: 0xFE000000)],
65-
1: [.P13: RaspberryPWM(gpioId: 13, alt: 0, channel:1, baseAddr: 0xFE000000), .P19: RaspberryPWM(gpioId: 19, alt: 5, channel:1, baseAddr: 0xFE000000)]
68+
0: [.P12: RaspberryPWM(gpioId: 12, alt: 0, channel:0, phy: RaspberryPHY.phyFor(baseAddr: 0xFE000000)),
69+
.P18: RaspberryPWM(gpioId: 18, alt: 5, channel:0, phy: RaspberryPHY.phyFor(baseAddr: 0xFE000000))],
70+
1: [.P13: RaspberryPWM(gpioId: 13, alt: 0, channel:1, phy: RaspberryPHY.phyFor(baseAddr: 0xFE000000)),
71+
.P19: RaspberryPWM(gpioId: 19, alt: 5, channel:1, phy: RaspberryPHY.phyFor(baseAddr: 0xFE000000))]
6672
]
6773
}
6874

@@ -79,11 +85,21 @@ public protocol PWMOutput {
7985
func cleanupPattern()
8086
}
8187

82-
public class RaspberryPWM: PWMOutput {
83-
let gpioId: UInt
84-
let alt: UInt
85-
let channel: Int
86-
let pwmdma: Int
88+
final public class RaspberryPHY {
89+
private static var existingPhy: RaspberryPHY? = nil
90+
91+
static func phyFor(baseAddr: UInt) -> RaspberryPHY {
92+
// If the physical base has changed, throw away the existing cache.
93+
// This generally shouldn't happen, as the base address is fixed.
94+
// It is reasonable to create a new instance if there is a mismatch, though.
95+
if let phy = existingPhy, phy.BCM2708_PERI_BASE == baseAddr {
96+
return phy
97+
}
98+
99+
let phy = RaspberryPHY(baseAddr: baseAddr)
100+
existingPhy = phy
101+
return phy
102+
}
87103

88104
let BCM2708_PERI_BASE: UInt
89105
let GPIO_BASE: UInt // GPIO Register
@@ -95,32 +111,22 @@ public class RaspberryPWM: PWMOutput {
95111
var gpioBasePointer: UnsafeMutablePointer<UInt32>!
96112
var pwmBasePointer: UnsafeMutablePointer<UInt32>!
97113
var clockBasePointer: UnsafeMutablePointer<UInt32>!
98-
var dmaBasePointer: UnsafeMutablePointer<UInt32>!
99-
var dmaCallbackPointer: UnsafeMutablePointer<DMACallback>! = nil
100-
var pwmRawPointer: UnsafeMutablePointer<UInt32>! = nil
101-
var mailbox: MailBox! = nil
114+
var dmaBasePointers: [UnsafeMutablePointer<UInt32>]! = []
102115

103-
var zeroPattern: Int = 0
104-
var onePattern: Int = 0
105-
var symbolBits: Int = 0
106-
var patternFrequency: Int = 0
107-
var patternDelay: Int = 0
108-
var dataLength: Int = 0
109-
110-
public init(gpioId: UInt, alt: UInt, channel: Int, baseAddr: UInt, dmanum: Int = 5) {
111-
self.gpioId = gpioId
112-
self.alt = alt
113-
self.channel = channel
114-
self.pwmdma = dmanum
116+
public private(set) var isInitialized: Bool = false
117+
public private(set) var isReadyForPwm: Bool = false
118+
119+
public init(baseAddr: UInt) {
115120
BCM2708_PERI_BASE = baseAddr
116121
GPIO_BASE = BCM2708_PERI_BASE + 0x200000 // GPIO Register
117122
PWM_BASE = BCM2708_PERI_BASE + 0x20C000 // PWM Register
118123
CLOCK_BASE = BCM2708_PERI_BASE + 0x101000 // Clock Manager Register
119124
PWM_PHY_BASE = BCM2708_PHY_BASE + 0x20C000 // PWM controller physical address
120125
}
121126

122-
/// Init PWM on this pin, set alternative function
123-
public func initPWM() {
127+
func initPhy() {
128+
guard !isInitialized else { return }
129+
124130
var mem_fd: Int32 = 0
125131

126132
//The only mem device that support PWM is /dev/mem
@@ -130,28 +136,103 @@ public class RaspberryPWM: PWMOutput {
130136
fatalError("Can't open /dev/mem , use sudo!")
131137
}
132138

133-
gpioBasePointer = memmap(from: mem_fd, at: GPIO_BASE)
134-
pwmBasePointer = memmap(from: mem_fd, at: PWM_BASE)
135-
clockBasePointer = memmap(from: mem_fd, at: CLOCK_BASE)
139+
gpioBasePointer = memmap(from: mem_fd, at: Int(GPIO_BASE))
140+
pwmBasePointer = memmap(from: mem_fd, at: Int(PWM_BASE))
141+
clockBasePointer = memmap(from: mem_fd, at: Int(CLOCK_BASE))
136142

137143
let DMAOffsets: [UInt] = [0x00007000, 0x00007100, 0x00007200, 0x00007300,
138-
0x00007400, 0x00007500, 0x00007600, 0x00007700,
139-
0x00007800, 0x00007900, 0x00007a00, 0x00007b00,
140-
0x00007c00, 0x00007d00, 0x00007e00, 0x00e05000]
144+
0x00007400, 0x00007500, 0x00007600, 0x00007700,
145+
0x00007800, 0x00007900, 0x00007a00, 0x00007b00,
146+
0x00007c00, 0x00007d00, 0x00007e00, 0x00e05000]
141147

142-
func dmanumToPhysicalAddress(_ dmanum: Int) -> UInt {
143-
guard dmanum < DMAOffsets.count else { return 0 }
144-
return BCM2708_PERI_BASE + DMAOffsets[dmanum]
148+
for dma_addr in DMAOffsets.map({ BCM2708_PERI_BASE + $0 }) {
149+
let pageOffset = Int(dma_addr % UInt(PAGE_SIZE))
150+
let adjusted_dma_addr = Int(dma_addr) - pageOffset
151+
let dma_map = UnsafeMutableRawPointer(memmap(from: mem_fd, at: adjusted_dma_addr))
152+
153+
let dmaBasePointer = (dma_map + pageOffset).assumingMemoryBound(to: UInt32.self)
154+
dmaBasePointers.append(dmaBasePointer)
145155
}
146156

147-
var dma_addr = dmanumToPhysicalAddress(pwmdma) // Address of a specific DMA Channel registers set
148-
let pageOffset = Int( dma_addr % UInt(PAGE_SIZE) )
149-
dma_addr -= UInt(pageOffset)
157+
close(mem_fd)
158+
}
150159

151-
let dma_map = UnsafeMutableRawPointer(memmap(from: mem_fd, at: dma_addr))
152-
dmaBasePointer = (dma_map + pageOffset).assumingMemoryBound(to: UInt32.self)
160+
func setPwmClock() {
161+
guard !isReadyForPwm else { return }
153162

154-
close(mem_fd)
163+
killClock()
164+
165+
let idiv = UInt32(2) // 250Mhz base frequency
166+
// Configure the clock and divisor that will be used to generate the signal
167+
clockBasePointer.advanced(by: 41).pointee = CLKM_PASSWD | (idiv << CLKM_DIV_DIVI) //CM CTL DIV register: Set DIVI value
168+
clockBasePointer.advanced(by: 40).pointee = CLKM_PASSWD | CLKM_CTL_ENAB | CLKM_CTL_SRC_PLLD //CM CTL register: Enable clock, MASH 0, source PLLD
169+
pwmBasePointer.pointee = 0 //PWM CTL register: Everything at 0, enable flag included, disables previous PWM
170+
usleep(10)
171+
172+
isReadyForPwm = true
173+
}
174+
175+
func killClock() {
176+
clockBasePointer.advanced(by: 40).pointee = CLKM_PASSWD | CLKM_CTL_KILL //CM CTL register: Set KILL flag
177+
usleep(10)
178+
179+
isReadyForPwm = false
180+
}
181+
182+
/// Maps a block of memory and returns the pointer
183+
internal func memmap(from mem_fd: Int32, at offset: Int) -> UnsafeMutablePointer<UInt32> {
184+
let m = mmap(
185+
nil, //Any adddress in our space will do
186+
PAGE_SIZE, //Map length
187+
PROT_READ|PROT_WRITE, // Enable reading & writting to mapped memory
188+
MAP_SHARED, //Shared with other processes
189+
mem_fd, //File to map
190+
off_t(offset) //Offset to GPIO peripheral
191+
)!
192+
193+
if (Int(bitPattern: m) == -1) { //MAP_FAILED not available, but its value is (void*)-1
194+
perror("mmap error")
195+
abort()
196+
}
197+
let pointer = m.assumingMemoryBound(to: UInt32.self)
198+
199+
return pointer
200+
}
201+
}
202+
203+
public class RaspberryPWM: PWMOutput {
204+
let gpioId: UInt
205+
let alt: UInt
206+
let channel: Int
207+
let pwmdma: Int
208+
209+
let phy: RaspberryPHY
210+
211+
var dmaBasePointer: UnsafeMutablePointer<UInt32>!
212+
var dmaCallbackPointer: UnsafeMutablePointer<DMACallback>! = nil
213+
var pwmRawPointer: UnsafeMutablePointer<UInt32>! = nil
214+
var mailbox: MailBox! = nil
215+
216+
var zeroPattern: Int = 0
217+
var onePattern: Int = 0
218+
var symbolBits: Int = 0
219+
var patternFrequency: Int = 0
220+
var patternDelay: Int = 0
221+
var dataLength: Int = 0
222+
223+
public init(gpioId: UInt, alt: UInt, channel: Int, phy: RaspberryPHY, dmanum: Int = 5) {
224+
self.phy = phy
225+
self.gpioId = gpioId
226+
self.alt = alt
227+
self.channel = channel
228+
self.pwmdma = dmanum
229+
230+
}
231+
232+
/// Init PWM on this pin, set alternative function
233+
public func initPWM() {
234+
phy.initPhy()
235+
dmaBasePointer = phy.dmaBasePointers[pwmdma]
155236

156237
// set PWM alternate function for this GPIO
157238
setAlt()
@@ -161,34 +242,36 @@ public class RaspberryPWM: PWMOutput {
161242
/// The signal starts, asynchronously(manged by a device external to the CPU), once this method is called and
162243
/// needs to be stopped manually calling `stopPWM()`.
163244
public func startPWM(period ns: Int, duty percent: Float) {
164-
// Kill the clock
165-
clockBasePointer.advanced(by: 40).pointee = CLKM_PASSWD | CLKM_CTL_KILL //CM CTL register: Set KILL flag
166-
usleep(10)
245+
if !phy.isReadyForPwm {
246+
phy.setPwmClock()
247+
}
167248

168-
// If the required frequency is too high, this value reduces the number of samples (scale does the opposite)
169-
let highFreqSampleReduction: UInt32 = (ns < 750) ? 10 : 1
249+
// @ 250Mhz, one slot in the PWM channel is 4 ns. All valid periods are multiples of that.
250+
// Pick a range that is at least as large as requested that we can represent, and round the
251+
// desired duty cycle to the nearest whole slot.
252+
//
253+
// Double precision is used here to handle very low frequencies in the range of <15Hz.
254+
// Otherwise we lose precision calculating data.
255+
let range = UInt32(max((Double(ns) / 4.0).rounded(.awayFromZero), 1))
170256

171-
let freq = UInt32( (1_000_000_000/UInt(ns)) * 100 / UInt(highFreqSampleReduction) )
172-
let clkSrc: ClockSource = (BCM2708_PERI_BASE != UInt(0xFE000000)) ? .PLLD : .PLLDPi4 //Differend Oscillator/PLLD freq for Pi4
173-
let (idiv, scale) = calculateDIVI(base: clkSrc, desired: UInt32(freq)) //Using the faster (with known freq) available clock to reduce jitter
257+
let data = min(UInt32((Double(percent) * Double(range) / 100.0).rounded(.toNearestOrAwayFromZero)), range)
174258

175-
// Configure the clock and divisor that will be used to generate the signal
176-
clockBasePointer.advanced(by: 41).pointee = CLKM_PASSWD | (idiv << CLKM_DIV_DIVI) //CM CTL DIV register: Set DIVI value
177-
clockBasePointer.advanced(by: 40).pointee = CLKM_PASSWD | CLKM_CTL_ENAB | CLKM_CTL_SRC_PLLD //CM CTL register: Enable clock, MASH 0, source PLLD
178-
pwmBasePointer.pointee = 0 //PWM CTL register: Everything at 0, enable flag included, disables previous PWM
179-
usleep(10)
180259
// Configure the parameters for the M/S algorithm, S the number of total slots in RNG1 and M the number of slots with high value in DAT1
181260
let RNG = (channel == 0) ? 4 : 8
182261
let DAT = (channel == 0) ? 5 : 9
183-
pwmBasePointer.advanced(by: RNG).pointee = 100 * scale / highFreqSampleReduction //RNG1 register
184-
pwmBasePointer.advanced(by: DAT).pointee = UInt32((percent / Float(highFreqSampleReduction)) * Float(scale)) //DAT1 register
262+
phy.pwmBasePointer.advanced(by: RNG).pointee = range //RNG1 register
263+
phy.pwmBasePointer.advanced(by: DAT).pointee = data //DAT1 register
185264
let PWMCTL_MSEN = (channel == 0) ? PWMCTL_MSEN1 : PWMCTL_MSEN2
186265
let PWMCTL_PWEN = (channel == 0) ? PWMCTL_PWEN1 : PWMCTL_PWEN2
187-
pwmBasePointer.pointee = PWMCTL_MSEN | PWMCTL_PWEN //PWM CTL register, channel enabled, M/S mode
266+
267+
phy.pwmBasePointer.pointee |= PWMCTL_MSEN | PWMCTL_PWEN //PWM CTL register, channel enabled, M/S mode
188268
}
189269

190270
public func stopPWM() {
191-
pwmBasePointer.pointee = 0 //PWM CTL register, everything at 0, enable flag included
271+
let PWMCTL_MSEN = (channel == 0) ? PWMCTL_MSEN1 : PWMCTL_MSEN2
272+
let PWMCTL_PWEN = (channel == 0) ? PWMCTL_PWEN1 : PWMCTL_PWEN2
273+
274+
phy.pwmBasePointer.pointee &= ~PWMCTL_MSEN & ~PWMCTL_PWEN //PWM CTL register, clear just the channel.
192275
}
193276

194277
/// Maps a block of memory and returns the pointer
@@ -214,7 +297,7 @@ public class RaspberryPWM: PWMOutput {
214297
/// Set the alternative function for this GPIO
215298
internal func setAlt() {
216299
let altid = (self.alt<=3) ? UInt32(self.alt+4) : self.alt==4 ? UInt32(3) : UInt32(2)
217-
let ptr = gpioBasePointer.advanced(by: Int(gpioId/10)) // GPFSELn 0..5
300+
let ptr = phy.gpioBasePointer.advanced(by: Int(gpioId/10)) // GPFSELn 0..5
218301
ptr.pointee &= ~(7<<((UInt32(gpioId)%10)*3))
219302
ptr.pointee |= (altid<<((UInt32(gpioId)%10)*3))
220303
}
@@ -305,11 +388,12 @@ extension RaspberryPWM {
305388
public func cleanupPattern() {
306389
dma_wait()
307390
// Stop the PWM
308-
pwmBasePointer.pointee = 0
391+
phy.pwmBasePointer.pointee = 0
309392
usleep(10)
310393
// Stop the clock killing the clock
311-
clockBasePointer.advanced(by: 40).pointee = CLKM_PASSWD | CLKM_CTL_KILL //Set KILL flag
394+
phy.clockBasePointer.advanced(by: 40).pointee = CLKM_PASSWD | CLKM_CTL_KILL //Set KILL flag
312395
usleep(10)
396+
phy.killClock()
313397

314398
mailbox.cleanup()
315399
}
@@ -337,7 +421,7 @@ extension RaspberryPWM {
337421

338422
// Round up to page size multiple
339423
let mboxsize = (size + PAGE_SIZE - 1) & ~(PAGE_SIZE - 1)
340-
mailbox = MailBox(handle: -1, size: mboxsize, isRaspi2: BCM2708_PERI_BASE != 0x20000000)
424+
mailbox = MailBox(handle: -1, size: mboxsize, isRaspi2: phy.BCM2708_PERI_BASE != 0x20000000)
341425

342426
guard let mailbox = mailbox else {fatalError("Could allocate mailbox.")}
343427

@@ -351,33 +435,33 @@ extension RaspberryPWM {
351435
}
352436

353437
// Stop the PWM
354-
pwmBasePointer.pointee = 0
438+
phy.pwmBasePointer.pointee = 0
355439
usleep(10)
356440
// Stop the clock killing the clock
357-
clockBasePointer.advanced(by: 40).pointee = CLKM_PASSWD | CLKM_CTL_KILL //Set KILL flag
441+
phy.killClock()
358442
usleep(10)
359443
// Check the BUSY flag, doesn't always work
360444
//while (clockBasePointer.advanced(by: 40).pointee & (1 << 7)) != 0 {}
361445

362446
// Configure clock
363-
let clkSrc: ClockSource = (BCM2708_PERI_BASE != UInt(0xFE000000)) ? .PLLD : .PLLDPi4 //Differend Oscillator/PLLD freq for Pi4
447+
let clkSrc: ClockSource = (phy.BCM2708_PERI_BASE != UInt(0xFE000000)) ? .PLLD : .PLLDPi4 //Differend Oscillator/PLLD freq for Pi4
364448
let idiv = calculateUnscaledDIVI(base: clkSrc, desired: UInt32(symbolBits * patternFrequency))
365-
clockBasePointer.advanced(by: 41).pointee = CLKM_PASSWD | (idiv << CLKM_DIV_DIVI) //Set DIVI value
366-
clockBasePointer.advanced(by: 40).pointee = CLKM_PASSWD | CLKM_CTL_ENAB | CLKM_CTL_SRC_PLLD //Enable clock, MASH 0, source PLLD
449+
phy.clockBasePointer.advanced(by: 41).pointee = CLKM_PASSWD | (idiv << CLKM_DIV_DIVI) //Set DIVI value
450+
phy.clockBasePointer.advanced(by: 40).pointee = CLKM_PASSWD | CLKM_CTL_ENAB | CLKM_CTL_SRC_PLLD //Enable clock, MASH 0, source PLLD
367451
usleep(10)
368452
// Check the BUSY flag, doesn't always work
369453
//while (clockBasePointer.advanced(by: 40).pointee & (1 << 7)) != 0 {}
370454

371455
// Configure PWM
372-
pwmBasePointer.advanced(by: 4).pointee = 32 // RNG1: 32-bits per word to serialize
456+
phy.pwmBasePointer.advanced(by: 4).pointee = 32 // RNG1: 32-bits per word to serialize
373457
usleep(10)
374-
pwmBasePointer.pointee = PWMCTL_CLRF1
458+
phy.pwmBasePointer.pointee = PWMCTL_CLRF1
375459
usleep(10)
376-
pwmBasePointer.advanced(by: 2).pointee = PWMDMAC_ENAB | 7 << PWMDMAC_PANIC | 3 << PWMDMAC_DREQ
460+
phy.pwmBasePointer.advanced(by: 2).pointee = PWMDMAC_ENAB | 7 << PWMDMAC_PANIC | 3 << PWMDMAC_DREQ
377461
usleep(10)
378-
pwmBasePointer.pointee = PWMCTL_USEF1 | PWMCTL_MODE1 //| PWMCTL_USEF2 | PWMCTL_MODE2 // For 2nd chan
462+
phy.pwmBasePointer.pointee = PWMCTL_USEF1 | PWMCTL_MODE1 //| PWMCTL_USEF2 | PWMCTL_MODE2 // For 2nd chan
379463
usleep(10)
380-
pwmBasePointer.pointee |= PWMCTL_PWEN1 //| PWMCTL_PWEN2 // For 2nd chan
464+
phy.pwmBasePointer.pointee |= PWMCTL_PWEN1 //| PWMCTL_PWEN2 // For 2nd chan
381465

382466
// Initialize the DMA control block
383467
dmaCallbackPointer.pointee = DMACallback(
@@ -387,7 +471,7 @@ extension RaspberryPWM {
387471
UInt32(0x5 << DMATI_PERMAP) | // PWM peripheral
388472
DMATI_SRC_INC, // Increment src addr
389473
source_ad: UInt32(mailbox.virtualTobaseBusAddress(UnsafeMutableRawPointer(pwmRawPointer))),
390-
dest_ad: UInt32(PWM_PHY_BASE + 0x18), // PWM FIF1 Register, shared between channels
474+
dest_ad: UInt32(phy.PWM_PHY_BASE + 0x18), // PWM FIF1 Register, shared between channels
391475
txfr_len: UInt32(dataSize),
392476
stride: 0,
393477
nextconbk: 0)

0 commit comments

Comments
 (0)