linux: RCInput_RPI RPI-4 Support

libraries/AP_HAL_Linux/RCInput_RPI.cpp

@@ -102,8 +102,19 @@ static uint16_t RcChnGpioTbl[RCIN_RPI_CHN_NUM] = {
 #define RCIN_RPI_PCM_INT_STC_A   (0x1c/4)
 #define RCIN_RPI_PCM_GRAY        (0x20/4)
 
-#define RCIN_RPI_PCMCLK_CNTL     38
-#define RCIN_RPI_PCMCLK_DIV      39
+#define RCIN_RPI_PCMCLK_CNTL     0x26
+#define RCIN_RPI_PCMCLK_DIV      0x27
+
+#define RCIN_RPI_PLL_CLK         50000
+/*
+*  Defaults to 107.14MHz in the firmware setup. 
+*  The PLL channel that used to generate the 100MHz now runs at 750MHz instead of 500,
+*  so the fw uses 750/7.
+*
+*  see: https://www.raspberrypi.org/forums/viewtopic.php?t=251902
+*/
+#define RCIN_RPI4_PLL_CLK        70000
+
 
 
 extern const AP_HAL::HAL& hal;
@@ -355,23 +366,63 @@ void RCInput_RPI::init_ctrl_data()
     ((dma_cb_t *)con_blocks->get_page(con_blocks->_virt_pages, cbp))->next = (uintptr_t)con_blocks->get_page(con_blocks->_phys_pages, 0);
 }
 
+void RCInput_RPI::init_PCM()
+{
+    if (_version != 4)
+    {
+        init_PCM_BCM2835();
+    }
+    else
+    {
+        init_PCM_BCM2711();
+    }
+}
+
 /*Initialise PCM
   See BCM2835 documentation:
   http://www.raspberrypi.org/wp-content/uploads/2012/02/BCM2835-ARM-Peripherals.pdf
  */
-void RCInput_RPI::init_PCM()
+void RCInput_RPI::init_PCM_BCM2835()
 {
     pcm_reg[RCIN_RPI_PCM_CS_A] = 1;                                          // Disable Rx+Tx, Enable PCM block
     hal.scheduler->delay_microseconds(100);
     clk_reg[RCIN_RPI_PCMCLK_CNTL] = 0x5A000006;                              // Source=PLLD (500MHz)
     hal.scheduler->delay_microseconds(100);
-    clk_reg[RCIN_RPI_PCMCLK_DIV] = 0x5A000000 | ((50000/RCIN_RPI_SAMPLE_FREQ)<<12);   // Set pcm div. If we need to configure DMA frequency.
+    clk_reg[RCIN_RPI_PCMCLK_DIV] = 0x5A000000 | ((RCIN_RPI_PLL_CLK/RCIN_RPI_SAMPLE_FREQ)<<12);   // Set pcm div. If we need to configure DMA frequency.
+    hal.scheduler->delay_microseconds(100);
+    clk_reg[RCIN_RPI_PCMCLK_CNTL] = 0x5A000016;                              // Source=PLLD and enable
+    hal.scheduler->delay_microseconds(100);
+    pcm_reg[RCIN_RPI_PCM_TXC_A] = 0<<31 | 1<<30 | 0<<20 | 0<<16;             // 1 channel, 8 bits
+    hal.scheduler->delay_microseconds(100);
+    pcm_reg[RCIN_RPI_PCM_MODE_A] = (10 - 1) << 10;                           //PCM mode 0b0010010000000000
+    hal.scheduler->delay_microseconds(100);
+    pcm_reg[RCIN_RPI_PCM_CS_A] |= 1<<4 | 1<<3;                               // Clear FIFOs
+    hal.scheduler->delay_microseconds(100);
+    pcm_reg[RCIN_RPI_PCM_DREQ_A] = 64<<24 | 64<<8;                           // DMA Req when one slot is free?
+    hal.scheduler->delay_microseconds(100);
+    pcm_reg[RCIN_RPI_PCM_CS_A] |= 1<<9;                                      // Enable DMA
+    hal.scheduler->delay_microseconds(100);
+    pcm_reg[RCIN_RPI_PCM_CS_A] |= 1<<2;                                      // Enable Tx
+    hal.scheduler->delay_microseconds(100);
+}
+
+
+/*Initialise PCM
+  See BCM2711 documentation:
+ */
+void RCInput_RPI::init_PCM_BCM2711()
+{
+    pcm_reg[RCIN_RPI_PCM_CS_A] = 1;                                          // Disable Rx+Tx, Enable PCM block
+    hal.scheduler->delay_microseconds(100);
+    clk_reg[RCIN_RPI_PCMCLK_CNTL] = 0x5A000006;                              // Source=PLLD (700MHz) // https://www.raspberrypi.org/forums/viewtopic.php?t=251902
+    hal.scheduler->delay_microseconds(100);
+    clk_reg[RCIN_RPI_PCMCLK_DIV] = 0x5A000000 | ((RCIN_RPI4_PLL_CLK/RCIN_RPI_SAMPLE_FREQ)<< 12); // 0x5A001900; //0x0x5A001900; //0x5A000000 | ((50000/RCIN_RPI_SAMPLE_FREQ)<< 4);   // Set pcm div. If we need to configure DMA frequency.
     hal.scheduler->delay_microseconds(100);
     clk_reg[RCIN_RPI_PCMCLK_CNTL] = 0x5A000016;                              // Source=PLLD and enable
     hal.scheduler->delay_microseconds(100);
     pcm_reg[RCIN_RPI_PCM_TXC_A] = 0<<31 | 1<<30 | 0<<20 | 0<<16;             // 1 channel, 8 bits
     hal.scheduler->delay_microseconds(100);
-    pcm_reg[RCIN_RPI_PCM_MODE_A] = (10 - 1) << 10;                           //PCM mode
+    pcm_reg[RCIN_RPI_PCM_MODE_A] = (10 - 1) << 10;                           //PCM mode 0b0010010000000000
     hal.scheduler->delay_microseconds(100);
     pcm_reg[RCIN_RPI_PCM_CS_A] |= 1<<4 | 1<<3;                               // Clear FIFOs
     hal.scheduler->delay_microseconds(100);

libraries/AP_HAL_Linux/RCInput_RPI.h

@@ -131,12 +131,15 @@ private:
     } rc_channels[RCIN_RPI_CHN_NUM];
 
     bool _initialized = false;
-
+    int _version =0;
+    
     void init_dma_cb(dma_cb_t** cbp, uint32_t mode, uint32_t source, uint32_t dest, uint32_t length, uint32_t stride, uint32_t next_cb);
     void* map_peripheral(uint32_t base, uint32_t len);
     void init_registers();
     void init_ctrl_data();
     void init_PCM();
+    void init_PCM_BCM2835();
+    void init_PCM_BCM2711();
     void init_DMA();
     void init_buffer();
     static void stop_dma();