Delaying a message sent using DMA

[ajc225]

I have a three teensy 4.1 setup below.



The idea is that teensy1 sends a packages filled with data to teensy2 and vice versa using DMA. I'm adding a third, middle teensy to act as a delayer to delay (a chosen amount of time z) the packages sent between the two teensys. To delay the packages, I was thinking of putting the received packages in a queue and timestamping when the packages were received. When the chosen amount of delay has passed, I then upload the package into a send array and send it to the other teensy. I've tested the setup and code (using dummy data variables) with teensy 1 and teensy 2 connected directly (no middle teensy) and the code works. However when I add the middle teensy, my data seems to stop in the middle and none of the packages or queues get updated. In the mid teensy code, you can see that I put a bunch of print statements, but the code stops after the first print statement on the very first line of the loop. I don't really understand why this is happening. I'm not sure if I set up the DMA linking incorrectly. Any advice or debugging suggestions would be appreciated.

Here's the main code and possible snippets of the from the included files I thought might helpful. Let me know if there is anything I might be missing.

Teensy 1 and 2 code (you may have seen a version of this on another thread of mine)

#include "DMASensor.h"
#include "DMAnChannel.h"
#include "DMAUART.h"
#include "Avatar_pinmap.h"
#include "Avatar_params.h"
#define sButton 33

#define LABELPRINT(label, value) Serial.print(label); Serial.print(": "); Serial.println(value);
#define PRINT(msg) Serial.println(msg);

DMAUART uart_enc, uart_link, uart_state;
DMAnChannel dma_link_s, dma_link_r, dma_state_s, dma_state_r;

uint32_t foo = 0xFFFFFFFF;

const uint8_t encoder_command = 0x54;
volatile uint16_t curr = 0;
volatile uint16_t rec_link[5];
volatile uint16_t snd_link[5];
int i = 0, j = 0, k=0, b=0;

void setup() {
  pinMode(sButton, INPUT);
  Serial.begin(115200);
  Serial.print("init done");
  Serial.println("Press button to start");
  while (digitalRead(sButton) == HIGH) {
    ;
  }
  // put your setup code here, to run once:
  // Configure GPT1 for encoder reading timestamping
  CCM_CCGR1 |= CCM_CCGR1_GPT1_BUS(CCM_CCGR_ON); // Enable clock to GPT1 module
  GPT1_CR = 0; // Disable for configuration
  GPT1_PR = 2400 - 1; // Prescale 24 MHz clock by 3 => 8 MHz
  GPT1_CR = GPT_CR_EN /* Enable timer */
            | GPT_CR_CLKSRC(1) /* 24 MHz peripheral clock as clock source */
            | GPT_CR_FRR /* Free-Run, do not reset */;

  snd_link[0] = 255;

  uart_link.init(LINK_CH, link_uart);

  // Init DMA channels that handle sending/recieving position data over the device link bus
  dma_link_s.init(0, snd_link, &(uart_link.ctrl->DATA), uart_link.dma_src_tx, link_send_tcd);
  dma_link_r.init(9, &(uart_link.ctrl->DATA), rec_link, uart_link.dma_src_rx, link_receive_tcd);
  dma_link_s.initPIT(2400);

  Serial.println("start");
}

void loop() {
  i = i + 1;
  k++;
  curr = currupdate();
  snd_link[4] = curr;
  if (i > 100) {
    i = 0;
  }
  if (rec_link[0] != 255||!checkPar(rec_link[4])) {
    uart_link.flushRX();
    j++;
  }
  LABELPRINT("PARITY FAILURES", j);
  Serial.print("record ");
  Serial.print(rec_link[0]);
  Serial.print(" ");
  Serial.print(rec_link[1]);
  Serial.print(" ");
  Serial.print(rec_link[2]);
  Serial.print(" ");
  Serial.print(rec_link[3]);
  Serial.print(" ");
  Serial.print(rec_link[4]);
  Serial.print(" ");
  Serial.print(rec_link[5]);
  Serial.println("");
  Serial.print("send ");
  Serial.print(snd_link[0]);
  Serial.print(" ");
  Serial.print(snd_link[1]);
  Serial.print(" ");
  Serial.print(snd_link[2]);
  Serial.print(" ");
  Serial.print(snd_link[3]);
  Serial.print(" ");
  Serial.print(snd_link[4]);
  Serial.print(" ");
  Serial.print(snd_link[5]);
  Serial.println("");
}

uint16_t currupdate() {
//  Serial.println("updating");
  float f = 1.15 + i * 0.01; //dynamic dummy variable
  int parity = 0;
  uint16_t f16,n;
  f16 = (uint16_t) (f * 1000);
  n = f16;
  while (n) {
      n &= (n - 1);
      parity ^= 1;
  }
  f16 =(uint16_t) f16<<1|parity;
  return f16;
}

boolean checkPar(uint16_t num){
  int pn, par;
  par = num&1;
  num>>=1;
  while (num) {
      num &= (num - 1);
      pn ^= 1;
  }
  return pn == par;
}

Mid Teensy code

#include "DMASensor.h"
#include "DMAnChannel.h"
#include "DMAUART.h"
#include "Avatar_pinmap.h"
#include "Avatar_params.h"
#define sButton 33
#define quesize 8192
#define delaytime 1000 // the time of delay is 550-100us(the rate
#define msgsize 5

#define LABELPRINT(label, value) Serial.print(label); Serial.print(": "); Serial.println(value);
#define PRINT(msg) Serial.println(msg);

// DMA channels and UART  
DMAUART uart_link2, uart_link1, uart_state1, uart_state2;
DMAnChannel dma_link1_s, dma_link1_r, dma_link2_s, dma_link2_r, dma_state1_s, dma_state1_r, dma_state2_s, dma_state2_r;

uint32_t foo = 0xFFFFFFFF;

// initialize arrays for recieve and sending 
volatile uint16_t rec_T1[msgsize] = {0};
volatile uint16_t snd_T2[msgsize] = {0};
volatile uint16_t rec_T2[msgsize] = {0};
volatile uint16_t snd_T1[msgsize] = {0};

//initilize queue for delayed messages 
volatile uint16_t delayque1[quesize][msgsize];
volatile uint16_t delayque2[quesize][msgsize];

//init time array to store corresponding delayque store time 
unsigned long timeque1[quesize], timeque2[quesize], st, e;

// init indexes and counters 
unsigned int indexk1, indexi1, indexk2, indexi2;
int i1 = 0, p1 = 0, k1 = 0, i2 = 0, p2 = 0, k2 = 0;

void setup() {
  // init delayques with 0
  for (int i = 0; i < quesize; i++) {
    for (int j = 0; j < msgsize; j++) {
      delayque1[i][j] = 0;
      delayque2[i][j] = 0;
    }
  }
  //send arrays first send 255
  snd_T2[0] = 255;
  snd_T1[0] = 255;

  //init button to sync all teensys
  pinMode(sButton, INPUT);
  Serial.begin(115200);
  Serial.print("init done");
  Serial.println("Press button to start");
  while (digitalRead(sButton) == HIGH) {
    ;
  }

  // init links uart channels
  //LINK1_CH is serial6, LINK2_CH is serial4
  uart_link1.init(LINK1_CH, link_uart);
  uart_link2.init(LINK2_CH, link_uart);

  //init links dma channel
  dma_link1_s.init(0, snd_T2, &(uart_link1.ctrl->DATA), uart_link1.dma_src_tx, link_send_tcd);
  dma_link1_r.init(5, &(uart_link1.ctrl->DATA), rec_T1, uart_link1.dma_src_rx, link_receive_tcd);
  dma_link2_s.init(1, snd_T1, &(uart_link2.ctrl->DATA), uart_link2.dma_src_tx, link_send_tcd);
  dma_link2_r.init(6, &(uart_link2.ctrl->DATA), rec_T2, uart_link2.dma_src_rx, link_receive_tcd);
  //  dma_curr_r.initPIT(2400);
  dma_link1_r.linkCh(dma_link2_s.ch);
  dma_link2_r.linkCh(dma_link1_s.ch);


  Serial.println("start");
}

void loop() {
  Serial.println("in loop");
  //checks if incomming messages are not scrambled 
  if (rec_T1[0] != 255 || !checkPar(rec_T1[4])) {
    uart_link1.flushRX();
    p1++;
    Serial.print("PARITY FAILURES: ");
    Serial.println(p1++);
  }
  else {
    //save incoming msg from link1 into delayque1
    Serial.println("store msg1");
    for (int z = 0; z < msgsize; z++) {
      delayque1[k1][z] = rec_T1[z];
    }
    //timestamping in timeque to cooresponding with delayque1 
    Serial.println("store time of msg1");
    timeque1[k1] = micros();
    indexk1++;
    k1 = indexk1 % quesize;
  }
  //checks if incomming messages are not scrambled
  if (rec_T2[0] != 255 || !checkPar(rec_T2[4])) {
    uart_link2.flushRX();
    p2++;
    Serial.print("PARITY FAILURES: ");
    Serial.println(p2++);
  }
  else {
     //save incoming msg from link2 into delayque2
    Serial.println("store msg2");
    //save rec_link in delayarray[k]
    for (int z = 0; z < msgsize; z++) {
      delayque2[k2][z] = rec_T2[z];
    }
    //timestamping in timeque to cooresponding with delayque2
    Serial.println("store time of msg2");
    timeque2[k2] = micros();
    indexk2++;
    k2 = indexk2 % quesize;
  }
  //checks if the the wanted delay has passed 
  if (micros() - timeque1[i1] >= delaytime) {
    //uploads the delayed msg into the send array to send through the other link
    Serial.println("upload delayed msg1 to channel");
    for (int z; z < msgsize; z++) {
      snd_T1[z] = delayque1[i1][z];
    }
    indexi1++;
    i1 = indexi1 % quesize;
  }
  //checks if the the wanted delay has passed 
  if (micros() - timeque2[i2] >= delaytime) {
    //uploads the delayed msg into the send array to send through the other link
    Serial.println("upload delayed msg2 to channel");
    for (int z; z < msgsize; z++) {
      snd_T1[z] = delayque1[i2][z];
    }
    indexi2++;
    i2 = indexi2 % quesize;
  }
}

boolean checkPar(uint16_t num) {
  int pn, par;
  par = num & 1;
  num >>= 1;
  while (num) {
    num &= (num - 1);
    pn ^= 1;
  }
  return pn == par;
}

.linkCh function

void DMAnChannel::linkCh(uint16_t link_ch) {

  if ((link_ch < 0) || (link_ch > 31)) {return;}   // channel range guard

  TCD->CSR &= ~(0b111111 << 7);                   // clear any previous channel linking and DONE flag
  TCD->CSR |= (link_ch & 0b11111) << 8;           // set new channel to link to
  TCD->CSR |= 0b1 << 5;                          // enable channel linking
}

link_uart LPUART

LPUART_t link_uart = {
  
      .GLOBAL = 0x0, // LPUART1_GLOBAL
      .PINCFG = 0x0,
      .BAUD = 0xBA00001,
      .STAT = 0xC00000,
      .CTRL = 0x300000,
      .DATA = 0x1000,
      .MATCH = 0x0,
      .MODIR = 0x6,
      .FIFO = 0xC10099,
      .WATER = 0x20000 //& (0b11 << 10)
};

TCD_ts

TCD_t link_receive_tcd = {        // DMA configuration for recieving position data over the LINK channel
  .SADDR = 0, // Source address
  .SOFF = 0, // Adjustment to the source address made after every minor loop iteration
  .ATTR = (0b000000 << 11) | (0b000 << 8) | (0b000000 << 3) | (0b000), // SMOD | SSIZE | DMOD | DSIZE
  .NBYTES = 2,  // Set number of bytes to transfer per minor loop iteration
  .SLAST = 0, // Source address adjustment after major loop completion
  .DADDR = 0,  // Destination address
  .DOFF = 1,
  .CITER = (0b0 << 15) | 5, // CITER and BITER must be the same when the TCD memory is loaded by software. CITER will decrement with each minor loop
  .DLASTSGA = -10,//check what this is 
  .CSR = (0b00 << 14) | (0b00000 << 8) | (0b0 << 5) | (0b0 << 4) | (0b0 << 3) | (0b0 << 2), // BWC | MAJORLINKCH | ESG | DREQ | INTHALF | INTMAJOR
  .BITER = (0b0 << 15) | 5 // Channel linking disabled | single service request (NO major looping)
};

TCD_t link_send_tcd = {        // DMA configuration for sending position data over the LINK channel
  .SADDR = 0, // Source address
  .SOFF = 1, // Adjustment to the source address made after every minor loop iteration
  .ATTR = (0b000000 << 11) | (0b000 << 8) | (0b000000 << 3) | (0b000), // SMOD | SSIZE | DMOD | DSIZE
  .NBYTES = 2,  // Set number of bytes to transfer per minor loop iteration
  .SLAST = -10, // Source address adjustment after major loop completion
  .DADDR = 0,  // Destination address
  .DOFF = 0,
  .CITER = (0b0 << 15) | 5, // CITER and BITER must be the same when the TCD memory is loaded by software. CITER will decrement with each minor loop
  .DLASTSGA = 0,
  .CSR = (0b00 << 14) | (0b00000 << 8) | (0b0 << 5) | (0b0 << 4) | (0b0 << 3) | (0b0 << 2), // BWC | MAJORLINKCH | ESG | DREQ | INTHALF | INTMAJOR
  .BITER = (0b0 << 15) | 5 // Channel linking disabled | single service request (NO major looping)
};