RoCoN: correct computation of channel location for case with multiple LXPWR slaves.

[fpga/lx-cpu1/lx-rocon.git] / sw / app / rocon / appl_pxmc.c

diff --git a/sw/app/rocon/appl_pxmc.c b/sw/app/rocon/appl_pxmc.c
index 7fc9fea6149d583b8556857f1f56fb718f60fa30..7d35393b20da1a78f1490ae8d2feccb578528b63 100644 (file)
--- a/sw/app/rocon/appl_pxmc.c
+++ b/sw/app/rocon/appl_pxmc.c
@@ -27,6 +27,8 @@
 #include <hal_machperiph.h>
 #include <stdlib.h>
 #include <string.h>
+#include <LPC17xx.h>
+#include <lpcTIM.h>
 
 #include "appl_defs.h"
 #include "appl_fpga.h"
@@ -234,7 +236,7 @@ pxmc_rocon_receiver_chan2reg(unsigned chan)
   rec_reg = fpga_lx_master_receiver_base;
 
  #ifdef LXPWR_WITH_SIROLADC
-  rec_reg += LX_MASTER_DATA_OFFS + 1 + (chan >> 8) * 3 + chan * 2;
+  rec_reg += LX_MASTER_DATA_OFFS + 1 + (chan >> 3) * 3 + chan * 2;
  #else /*LXPWR_WITH_SIROLADC*/
   rec_reg += LX_MASTER_DATA_OFFS + chan;
  #endif /*LXPWR_WITH_SIROLADC*/
@@ -307,7 +309,7 @@ pxmc_rocon_pwm_chan2reg(unsigned chan)
   pwm_reg = fpga_lx_master_transmitter_base;
 
  #ifdef LXPWR_WITH_SIROLADC
-  pwm_reg += LX_MASTER_DATA_OFFS + 1 + (chan >> 8) + chan;
+  pwm_reg += LX_MASTER_DATA_OFFS + 1 + (chan >> 3) + chan;
  #else /*LXPWR_WITH_SIROLADC*/
   pwm_reg += LX_MASTER_DATA_OFFS + chan;
  #endif /*LXPWR_WITH_SIROLADC*/
@@ -704,6 +706,100 @@ pxmc_rocon_pwm_dc_out(pxmc_state_t *mcs)
   return 0;
 }
 
+volatile void *pxmc_rocon_rx_data_hist_buff;
+volatile void *pxmc_rocon_rx_data_hist_buff_end;
+
+uint32_t pxmc_rocon_rx_last_irq;
+uint32_t pxmc_rocon_rx_cycle_time;
+uint32_t pxmc_rocon_rx_irq_latency;
+uint32_t pxmc_rocon_rx_irq_latency_max;
+
+IRQ_HANDLER_FNC(pxmc_rocon_rx_done_isr)
+{
+  uint32_t ir;
+
+  ir = ROCON_RX_TIM->IR & LPC_TIM_IR_ALL_m;
+  ROCON_RX_TIM->IR = ir;
+  if (ir & LPC_TIM_IR_CR1INT_m) {
+    uint32_t cr0, cr1;
+    cr0 = ROCON_RX_TIM->CR0;
+    cr1 = ROCON_RX_TIM->CR1;
+
+    pxmc_rocon_rx_cycle_time = cr1 - pxmc_rocon_rx_last_irq;
+    pxmc_rocon_rx_last_irq = cr1;
+
+    hal_gpio_set_value(T2MAT0_PIN, 1);
+    hal_gpio_set_value(T2MAT1_PIN, 0);
+    hal_gpio_set_value(T2MAT0_PIN, 0);
+
+    if (pxmc_rocon_rx_data_hist_buff >= pxmc_rocon_rx_data_hist_buff_end)
+      pxmc_rocon_rx_data_hist_buff = NULL;
+
+    if (pxmc_rocon_rx_data_hist_buff != NULL) {
+      int i;
+      volatile uint32_t *pwm_reg = fpga_lx_master_transmitter_base + 8;
+      volatile uint32_t *rec_reg = fpga_lx_master_receiver_base + 8;
+      uint16_t *pbuf = (uint16_t *)pxmc_rocon_rx_data_hist_buff;
+      for (i = 0; i < 8; i++) {
+        *(pbuf++) = *(rec_reg++);
+      }
+      for (i = 0; i < 8; i++) {
+        *(pbuf++) = *(pwm_reg++);
+      }
+      pxmc_rocon_rx_data_hist_buff = pbuf;
+    }
+
+    pxmc_rocon_rx_irq_latency = ROCON_RX_TIM->TC - cr1;
+    if (pxmc_rocon_rx_irq_latency > pxmc_rocon_rx_irq_latency_max)
+      pxmc_rocon_rx_irq_latency_max = pxmc_rocon_rx_irq_latency;
+
+   #ifdef PXMC_ROCON_TIMED_BY_RX_DONE
+    pxmc_sfi_isr();
+    do_pxmc_coordmv();
+   #endif /*PXMC_ROCON_TIMED_BY_RX_DONE*/
+  }
+
+  return IRQ_HANDLED;
+}
+
+int
+pxmc_rocon_rx_done_isr_setup(irq_handler_t rx_done_isr_handler)
+{
+
+  disable_irq(ROCON_RX_IRQn);
+
+  hal_pin_conf_set(T2MAT0_PIN, PORT_CONF_GPIO_OUT_LO);
+  hal_pin_conf_set(T2MAT1_PIN, PORT_CONF_GPIO_OUT_LO);
+  hal_pin_conf(T2CAP0_PIN);
+  hal_pin_conf(T2CAP1_PIN);
+
+  hal_gpio_direction_output(T2MAT0_PIN, 1);
+  hal_gpio_direction_output(T2MAT1_PIN, 0);
+  hal_gpio_set_value(T2MAT0_PIN, 0);
+
+  /* Enable CLKOUT pin function, source CCLK, divide by 1 */
+  LPC_SC->CLKOUTCFG = 0x0100;
+
+  request_irq(ROCON_RX_IRQn, rx_done_isr_handler, 0, NULL,NULL);
+
+  ROCON_RX_TIM->TCR = 0;
+  ROCON_RX_TIM->CTCR = 0;
+  ROCON_RX_TIM->PR = 0;        /* Divide by 1 */
+
+  ROCON_RX_TIM->CCR = LPC_TIM_CCR_CAP0RE_m | LPC_TIM_CCR_CAP1FE_m |
+                   LPC_TIM_CCR_CAP1I_m;
+
+  ROCON_RX_TIM->EMR = __val2mfld(LPC_TIM_EMR_EMC0_m, LPC_TIM_EMR_NOP) |
+                   __val2mfld(LPC_TIM_EMR_EMC1_m, LPC_TIM_EMR_NOP);
+
+  ROCON_RX_TIM->MCR = 0;                       /* No IRQ on MRx */
+  ROCON_RX_TIM->TCR = LPC_TIM_TCR_CEN_m;       /* Enable timer counting */
+  enable_irq(ROCON_RX_IRQn);           /* Enable interrupt */
+
+  return 0;
+
+}
+
 int
 pxmc_rocon_pwm_master_init(void)
 {
@@ -711,6 +807,7 @@ pxmc_rocon_pwm_master_init(void)
   int grp_in = 0;
   int grp_out = 0;
   unsigned word_slot;
+  unsigned receiver_done_div = 1;
  #ifdef LXPWR_WITH_SIROLADC
   unsigned lxpwr_header = 1;
   unsigned lxpwr_words = 1 + 8 * 2 + 2;
@@ -723,10 +820,14 @@ pxmc_rocon_pwm_master_init(void)
   unsigned lxpwr_chip_pwm_cnt = 8;
  #endif /*LXPWR_WITH_SIROLADC*/
 
+ #ifdef PXMC_ROCON_TIMED_BY_RX_DONE
+  receiver_done_div = 5;
+ #endif /*PXMC_ROCON_TIMED_BY_RX_DONE*/
+
   *fpga_lx_master_reset = 1;
   *fpga_lx_master_transmitter_reg = 0;
   *fpga_lx_master_transmitter_cycle = 2500; /* 50 MHz -> 20 kHz */
-  *fpga_lx_master_receiver_done_div = 1 << 8;
+  *fpga_lx_master_receiver_done_div = receiver_done_div << 8;
 
   for (i = 0; i < LX_MASTER_DATA_OFFS + lxpwr_words * lxpwr_chips; i++)
     fpga_lx_master_receiver_base[i] = 0;
@@ -754,7 +855,7 @@ pxmc_rocon_pwm_master_init(void)
 
   *fpga_lx_master_reset = 0;
   *fpga_lx_master_transmitter_cycle = 2500; /* 50 MHz -> 20 kHz */
-  *fpga_lx_master_receiver_done_div = 1 << 8;
+  *fpga_lx_master_receiver_done_div = receiver_done_div << 8;
 
   return 0;
 }
@@ -1695,6 +1796,9 @@ int pxmc_initialize(void)
   //pxmc_rocon_pwm3ph_wr(mcs, 0, 0, 0);
 
   pxmc_rocon_pwm_master_init();
+ #ifdef PXMC_ROCON_TIMED_BY_RX_DONE
+  pxmc_rocon_rx_done_isr_setup(pxmc_rocon_rx_done_isr);
+ #endif /*PXMC_ROCON_TIMED_BY_RX_DONE*/
 
   pxmc_main_list.pxml_cnt = 0;
   pxmc_dbg_hist = NULL;