--- a/modulator.c	Tue Feb 04 14:20:07 2025 +1030
+++ b/modulator.c	Sat Feb 15 22:57:18 2025 +1030
@@ -17,46 +17,259 @@
 **
 */
 
+#include <stdio.h>
+#include <string.h>
+#include "bitstring.h"
+
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wtype-limits"
+#pragma GCC diagnostic ignored "-Wsign-compare"
 #include "pico/stdlib.h"
 #include "hardware/clocks.h"
 #include "hardware/dma.h"
+#include "hardware/interp.h"
 #include "hardware/irq.h"
+#include "hardware/pll.h"
 #include "hardware/pio.h"
+#include "hardware/pwm.h"
+#include "hardware/structs/pll.h"
+#include "hardware/structs/clocks.h"
+#pragma GCC diagnostic pop
 
 #include "dac.pio.h"
 
-#if 0
-extern void* shaped_trap_dat_size;
-extern void* shaped_trap_dat_start;
+// https://github.com/howerj/q
+#include "q/q.h"
 
-extern void* sgauss_dat_size;
-extern void* sgauss_dat_start;
-#endif
-
-uint8_t shaped_trap_dat_start[] = { 0, 1, 2, 4, 8, 16, 32, 64, 128, 0, 0, 0, 0, 0, 0, 0 };
-//uint8_t shaped_trap_dat_start[] = { 0, 1, 2, 3, 1, 2, 0, 3, 1, 1, 2, 3, 0, 1, 2, 3 };
-#define SHAPED_TRAP_DAT_BITS 4
-_Static_assert((1 << SHAPED_TRAP_DAT_BITS) == sizeof(shaped_trap_dat_start));
+#include "shaped-trap.h"
 
 static int dma_chan;
+uint8_t pulse_data[65536];
+uint8_t pulse_ctrl[65536];
 
 void
 dma_handler(void) {
     // Clear the interrupt request.
     dma_hw->ints0 = 1u << dma_chan;
     // Give the channel a new wave table entry to read from, and re-trigger it
-    dma_channel_set_read_addr(dma_chan, shaped_trap_dat_start, true);
+    //dma_channel_set_read_addr(dma_chan, shaped_trap_dat_start, true);
+}
+
+unsigned slice_num = 0;
+
+void
+pwm_wrap(void) {
+  pwm_clear_irq(slice_num);
+#if 0
+  static unsigned state = 0;
+
+  gpio_put(PICO_DEFAULT_LED_PIN, state);
+  state = !state;
+#endif
+
+  // Trigger another pulse read out
+  dma_channel_set_read_addr(dma_chan, pulse_data, true);
+}
+
+// Calculate pulse shape data
+// TODO: sense, gate, phase, active, T/R switch
+// Could encode them as bit stream like data but more compact would be
+// (say) a list of counts to toggle pins at
+#define SENSE		0x01
+#define GATE		0x02
+#define PHINV		0x04
+#define PACTIVE		0x08
+
+// Need to add pre/postgate/sense/phase counters
+unsigned
+compute_pulse(uint8_t *data, uint8_t *ctrl, unsigned datalen, uint16_t plen, char *code, uint8_t ncode, const uint8_t *shape, uint8_t shapelen, uint8_t codegap, uint8_t slew1, uint8_t slew2, uint8_t dcofs) {
+  uint32_t shapesamples, nsamples, idx;
+  q_t dcscale, stepsize;
+  char tmps[20];
+
+  dcscale = qdiv(qsub(qint(255), qint(dcofs)), qint(255));
+  qsprint(dcscale, tmps, sizeof(tmps));
+  printf("dcscale = %s\n", tmps);
+
+  if (ncode == 1) {
+    // Number of samples for half of the pulse
+    // Do division first so we don't overflow Q16.16
+    shapesamples = qtoi(qmul(qdiv(qint(plen), qint(100)), qint(shapelen / 2)));
+    // Number of samples for everything
+    nsamples = shapesamples * 2 + slew1 + slew2;
+  } else {
+    shapesamples = plen / 2;
+    nsamples = shapesamples * 2 * ncode + codegap * (ncode - 1) + slew1 + slew2;
+  }
+
+  // Number of samples per step in the pulse shape
+  stepsize = qdiv(qint(shapesamples), qint(shapelen));
+  qsprint(stepsize, tmps, sizeof(tmps));
+  printf("shapelen = %d shapesamples = %lu nsamples = %lu stepsize = %s\n", shapelen, shapesamples, nsamples, tmps);
+
+  if (nsamples > datalen) {
+    printf("Pulse too long (%ld > %u)\n", nsamples, datalen);
+    return 0;
+  }
+  if (shapesamples < 2) {
+    printf("Pulse too short (%lu < %d)\n", shapesamples, 2);
+    return 0;
+  }
+  if (qtoi(shapesamples) > 65535) {
+    printf("Shape too long (%u > %d)\n", qtoi(shapesamples), 65535);
+    return 0;
+  }
+  idx = 0;
+
+  // Up slew
+  for (uint16_t i = 0; i < slew1; i++) {
+    data[idx++] = qtoi(qdiv(qmul(qint(dcofs), qint(i)), qint(slew1)));
+    ctrl[idx] |= PACTIVE;
+  }
+  for (uint16_t c = 0; c < ncode; c++) {
+    uint ctrltmp = PACTIVE;
+    if (code[c] == '0')
+      ctrltmp |= PHINV;
+
+    // Pulse up
+    for (uint16_t i = 0; i < shapesamples; i++) {
+      data[idx++] = dcofs + qtoi(qmul(qint(shape[qtoi(qdiv(qint(i), stepsize))]), dcscale));
+      ctrl[idx] = ctrltmp;
+    }
+    // Pulse down
+    for (uint16_t i = 0; i < shapesamples; i++) {
+      data[idx++] = dcofs + qtoi(qmul(qint(shape[qtoi(qdiv(qint(shapesamples - i - 1), stepsize))]), dcscale));
+      // Could replace this with a separate loop to poke it into place
+      // Similarly for TR switch when implemented
+      if (i == 0 && c == 0)
+	ctrl[idx] = ctrltmp | SENSE;
+      else
+	ctrl[idx] = ctrltmp;
+    }
+
+    // Code gap
+    if (c < ncode - 1)
+      for (uint16_t i = 0; i < codegap; i++) {
+	data[idx++] = dcofs;
+	ctrl[idx] = ctrltmp;
+      }
+  }
+
+  // Down slew
+  for (uint16_t i = 0; i < slew2; i++) {
+    data[idx++] = qtoi(qdiv(qmul(qint(dcofs), qint(slew2 - i)), qint(slew2)));
+    ctrl[idx] |= PACTIVE;
+  }
+  return idx - 1;
+}
+
+// As per hello_interp.c
+void interp_test(uint8_t *data, const uint8_t *shape) {
+  interp_config cfg;
+  const int uv_fractional_bits = 12;
+
+  // Step size is 0.25 (ie stretching by 4x)
+  unsigned step = (1 << uv_fractional_bits) / 4;
+
+  puts("interp_test\n");
+  // Setup lane 0 to generate index into shape table
+  cfg = interp_default_config();
+  interp_config_set_shift(&cfg, uv_fractional_bits);
+  interp_config_set_mask(&cfg, 0, 32 - uv_fractional_bits);
+  interp_config_set_blend(&cfg, true);
+  interp_set_config(interp0, 0, &cfg);
+
+  // Setup lane 1 to LERP each sample pair
+  // shift XXXX XXXX XXXX XXXX XXXX FFFF FFFF FFFF (accum 0 via cross input)
+  // to 0000 XXXX XXXX XXXX XXXX FFFF FFFF FFFF
+  cfg = interp_default_config();
+  interp_config_set_shift(&cfg, uv_fractional_bits - 8);
+  interp_config_set_signed(&cfg, false);
+  interp_config_set_cross_input(&cfg, true); // unsigned blending
+  interp_set_config(interp0, 1, &cfg);
+
+  interp0->accum[0] = 0; // Initial offset into shape table
+  interp0->base[2] = (uintptr_t)shape; // Start of shape table
+  for (unsigned i = 0; i < 122 * 4; i++) {
+    // Get sample pair
+    uint8_t *sample_pair = (uint8_t *) interp0->peek[2];
+    // Ask lane 1 for a LERP, using the lane 0 accumulator
+    interp0->base[0] = sample_pair[0];
+    interp0->base[1] = sample_pair[1];
+    uint32_t peek1 = interp0->peek[1];
+    uint32_t add_raw1 = interp0->add_raw[1];
+    data[i] = peek1;
+    printf("%d\t(%lu%% between %d and %d) %lu\n",
+	   (int)peek1,
+	   100 * (add_raw1 & 0xff) / 0xff,
+	   sample_pair[0], sample_pair[1],
+	   interp_get_accumulator(interp0, 0));
+    interp0->add_raw[0] = step;
+  }
+  puts("done");
 }
 
 int
 main(void) {
-    int i;
-    const uint LED_PIN = PICO_DEFAULT_LED_PIN;
+    absolute_time_t then, now;
+
+    // Set sysclk to 120MHz
+    set_sys_clock_khz(120000, true);
 
     stdio_init_all();
+    printf("\n\n\nIniting\n");
 
-    gpio_init(LED_PIN);
-    gpio_set_dir(LED_PIN, GPIO_OUT);
+    // Needed otherwise timer related functions hang under debugging
+    // https://github.com/raspberrypi/pico-sdk/issues/1152#issuecomment-1418248639
+    timer_hw->dbgpause = 0;
+
+    gpio_init(PICO_DEFAULT_LED_PIN);
+    gpio_set_dir(PICO_DEFAULT_LED_PIN, GPIO_OUT);
+
+    interp_config cfg = interp_default_config();
+    interp_config_set_blend(&cfg, true);
+    interp_set_config(interp0, 0, &cfg);
+
+    cfg = interp_default_config();
+    interp_set_config(interp0, 1, &cfg);
+
+    interp_test(pulse_data, shaped_trap);
+
+    uint32_t idx;
+    uint16_t plen = 4000;
+    char *code = "1110010";
+    //char *code = "10";
+    uint8_t codegap = 4;
+    uint8_t slew1 = 10;
+    uint8_t slew2 = 10;
+    uint8_t dcofs = 110;
+    memset(pulse_data, 0, sizeof(pulse_data));
+    memset(pulse_ctrl, 0, sizeof(pulse_ctrl));
+    then = get_absolute_time();
+    if ((idx = compute_pulse(pulse_data, pulse_ctrl, sizeof(pulse_data),
+			     plen, code, strlen(code),
+			     shaped_trap, sizeof(shaped_trap),
+			     codegap, slew1, slew2, dcofs)) == 0) {
+      printf("Failed to compute pulse\n");
+      while (1)
+	;
+    }
+    now = get_absolute_time();
+    printf("Pulse computation took %lld usec and created %lu samples\n", absolute_time_diff_us(then, now), idx);
+
+    pwm_config c = pwm_get_default_config();
+    // 120MHz / 250 = 480kHz base
+    // Maximum divisor is only 256 which limits the low end,
+    // could further subdivide in the IRQ handler
+    pwm_config_set_clkdiv_int(&c, 250);
+    // 8Hz
+    pwm_config_set_wrap(&c, 60000 - 1);
+    pwm_init(slice_num, &c, true);
+    pwm_clear_irq(slice_num);
+    pwm_set_irq_enabled(slice_num, true);
+    irq_set_exclusive_handler(PWM_IRQ_WRAP, pwm_wrap);
+    irq_set_enabled(PWM_IRQ_WRAP, true);
+    pwm_init(slice_num, &c, true);
 
     // Load the clocked_input program, and configure a free state machine
     // to run the program.
@@ -65,15 +278,17 @@
     uint sm = pio_claim_unused_sm(pio, true);
     // XXX: I would prefer starting at GPIO16 but in that case the top 2
     // bits don't seem to work
-    dac_program_init(pio, sm, offset, 14);
+    // Clock divisor of 2 so it runs at 60MHz and
+    // generates a 30MHz clock
+    dac_program_init(pio, sm, offset, 14, 2);
 
-// Configure a channel to write the same word (32 bits) repeatedly to PIO0
+    // Configure a channel to write the same word (8 bits) repeatedly to PIO0
     // SM0's TX FIFO, paced by the data request signal from that peripheral.
     dma_chan = dma_claim_unused_channel(true);
     dma_channel_config dmac = dma_channel_get_default_config(dma_chan);
+    // XXX: need to get pulse generator to pad to 32 bits
     channel_config_set_transfer_data_size(&dmac, DMA_SIZE_32);
     channel_config_set_read_increment(&dmac, true);
-    channel_config_set_ring(&dmac, false, SHAPED_TRAP_DAT_BITS);
     channel_config_set_dreq(&dmac, DREQ_PIO0_TX0);
 
     dma_channel_configure(
@@ -81,7 +296,7 @@
         &dmac,
         &pio0_hw->txf[0], // Write address (only need to set this once)
         NULL,             // Don't provide a read address yet
-        100, 		  // Write this many times then interrupt
+        idx >> 2, 	  // Transfer count
         false             // Don't start yet
     );
 
@@ -92,21 +307,15 @@
     irq_set_exclusive_handler(DMA_IRQ_0, dma_handler);
     irq_set_enabled(DMA_IRQ_0, true);
 
-    // Manually call the handler once, to trigger the first transfer
-    dma_handler();
-
     // Everything else from this point is interrupt-driven. The processor has
     // time to sit and think about its early retirement -- maybe open a bakery?
     while (true) {
-#if 0
-	for (int i = 0; i < sizeof(shaped_trap_dat_start) / 4; i++) {
-	    pio_sm_put_blocking(pio, sm, ((uint32_t *)shaped_trap_dat_start)[i]);
-	}
-#endif
-#if 0
-	gpio_put(LED_PIN, 1);
+      dma_channel_wait_for_finish_blocking(dma_chan);
+	gpio_put(PICO_DEFAULT_LED_PIN, 1);
+	sleep_ms(100);
+	gpio_put(PICO_DEFAULT_LED_PIN, 0);
 	sleep_ms(100);
-	gpio_put(LED_PIN, 0);
-#endif
     }
+
+    __breakpoint();
 }