// Bit-banged I2C, 3-wire SPI and CAN between two roles on one open-drain bus:
// D4 (GPIO6) is the data line and D5 (GPIO7) the clock line.
//
// Role A is the I2C/SPI master and a CAN node; role B is the I2C/SPI slave,
// driven entirely from GPIO edge interrupts, and the other CAN node. Boot
// straps pick the roles: D1 (GPIO3) low = A only, D10 (GPIO10) low = B only,
// neither = both on this chip (the single-board loopback). Each role only
// pulls a line low or releases it, and a line is low while either role on
// this chip pulls it: the same wired-AND two chips get from a real wire. The
// pads read back the real line, so a logic analyzer on D4/D5 sees the whole
// conversation.
//
// Every run prints "BB ..." result lines; the e2e scenario and the hardware
// capture compare them. Timing is deliberately irregular: bit half periods
// come from a fixed-seed PRNG, the I2C slave stretches the clock, the SPI data
// line turns around mid-frame with no chip select, and both CAN nodes start
// the same frame so arbitration is decided on a late ID bit.
#include <stdio.h>
#include <stdint.h>
#include <string.h>
#include "driver/gpio.h"
#include "esp_attr.h"
#include "esp_cpu.h"
#include "esp_rom_sys.h"
#include "esp_timer.h"
#include "esp_rom_gpio.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "soc/gpio_reg.h"

#define PIN_DATA 6 // D4
#define PIN_CLK 7  // D5
#define PIN_ONLY_A 3  // D1 strap
#define PIN_ONLY_B 10 // D10 strap
#define DATA 0
#define CLK 1
#define ROLE_A 0
#define ROLE_B 1
#define CYCLES_PER_US 160

static const int line_pin[2] = { PIN_DATA, PIN_CLK };
static volatile uint8_t want_low[2][2]; // [role][line]
static portMUX_TYPE line_mux = portMUX_INITIALIZER_UNLOCKED;

static inline uint32_t cycles(void) { return esp_cpu_get_cycle_count(); }

static IRAM_ATTR void wait_us(uint32_t us)
{
    uint32_t start = cycles();
    while(cycles() - start < us * CYCLES_PER_US) {
    }
}

static inline IRAM_ATTR void wait_until(uint32_t t)
{
    while((int32_t)(cycles() - t) < 0) {
    }
}

// With both roles on this chip, a released line is driven high instead of
// left to the internal pull-up: the bench's logic analyzer loads D5 harder
// than the ~45 kOhm pull-up, which then reads low. The line is still the
// wired-AND of both roles; only its high level comes from a driver.
static int drive_high;

// Pull a line low (low = 1) or release it for one role. The pad drives low
// while any role on this chip pulls; otherwise the pull-up (and the other
// chip) decides, or the pad drives high (drive_high).
static IRAM_ATTR void set_line(int role, int line, int low)
{
    portENTER_CRITICAL_SAFE(&line_mux);
    want_low[role][line] = low;
    uint32_t bit = 1u << line_pin[line];
    if(want_low[ROLE_A][line] || want_low[ROLE_B][line]) {
        REG_WRITE(GPIO_OUT_W1TC_REG, bit);
        REG_WRITE(GPIO_ENABLE_W1TS_REG, bit);
    } else if(drive_high) {
        REG_WRITE(GPIO_OUT_W1TS_REG, bit);
        REG_WRITE(GPIO_ENABLE_W1TS_REG, bit);
    } else {
        REG_WRITE(GPIO_ENABLE_W1TC_REG, bit);
    }
    portEXIT_CRITICAL_SAFE(&line_mux);
}

static inline IRAM_ATTR int read_line(int line)
{
    return (REG_READ(GPIO_IN_REG) >> line_pin[line]) & 1;
}

static uint32_t bench_i2c_cycles, bench_spi_cycles, bench_can1_cycles, bench_can2_cycles;

static uint32_t rng = 0x2545f491;
static uint32_t rnd(void)
{
    rng ^= rng << 13;
    rng ^= rng >> 17;
    rng ^= rng << 5;
    return rng;
}

// ---------------------------------------------------------------- role B (ISR)

enum { PHASE_I2C, PHASE_SPI, PHASE_CAN };
static volatile int b_phase = PHASE_I2C;

enum { I_IDLE, I_ADDR, I_RX, I_TX, I_IGNORE };
static int i_state = I_IDLE, i_bit, i_byte, i_rw, i_rx, i_tx, i_master_ack, i_switch;
static uint8_t b_regs[4];
static volatile int b_reads, b_stretches, b_writes;

static int64_t s_last_edge;
static int s_bit, s_frames, s_switch;
static uint32_t s_cmd, s_resp;
static uint8_t b_cmds[8];

static IRAM_ATTR void b_drive_bit(int value) { set_line(ROLE_B, DATA, !value); }

static IRAM_ATTR void i2c_scl_rising(int sda)
{
    switch(i_state) {
        case I_ADDR:
        case I_RX:
            if(i_bit < 8) {
                i_byte = (i_byte << 1) | sda;
                i_bit++;
            } else {
                i_bit = 9;
            }
            break;
        case I_TX:
            if(i_bit < 8) {
                i_bit++;
            } else {
                i_master_ack = sda == 0;
                i_bit = 9;
            }
            break;
    }
}

static IRAM_ATTR void i2c_scl_falling(void)
{
    switch(i_state) {
        case I_ADDR:
            if(i_bit == 8) {
                int address = i_byte >> 1;
                if(address == 0x3a || address == 0x3b) {
                    i_rw = i_byte & 1;
                    i_switch |= address == 0x3b;
                    b_drive_bit(0); // ACK
                } else {
                    i_state = I_IGNORE;
                }
            } else if(i_bit == 9) {
                // End of the address ACK: hold SCL low while "processing".
                set_line(ROLE_B, CLK, 1);
                b_drive_bit(1);
                if(i_rw) {
                    i_state = I_TX;
                    i_tx = 0;
                    i_byte = b_regs[0] ^ 0xff;
                    b_drive_bit((i_byte >> 7) & 1);
                } else {
                    i_state = I_RX;
                    i_byte = 0;
                }
                i_bit = 0;
                b_stretches++;
                wait_us(50);
                set_line(ROLE_B, CLK, 0);
            }
            break;
        case I_RX:
            if(i_bit == 8) {
                if(i_rx < 4) {
                    b_regs[i_rx] = i_byte;
                }
                i_rx++;
                b_writes++;
                b_drive_bit(0); // ACK
            } else if(i_bit == 9) {
                b_drive_bit(1);
                i_bit = 0;
                i_byte = 0;
            }
            break;
        case I_TX:
            if(i_bit >= 1 && i_bit <= 7) {
                b_drive_bit((i_byte >> (7 - i_bit)) & 1);
            } else if(i_bit == 8) {
                b_drive_bit(1); // the master ACKs
                b_reads++;
            } else if(i_bit == 9) {
                if(i_master_ack) {
                    i_tx++;
                    i_byte = b_regs[i_tx & 3] ^ 0xff;
                    i_bit = 0;
                    b_drive_bit((i_byte >> 7) & 1);
                } else {
                    i_state = I_IGNORE;
                }
            }
            break;
    }
}

static IRAM_ATTR uint32_t spi_response(uint32_t cmd)
{
    if(cmd == 0x9f) {
        return 0xc35a96;
    }
    if(cmd == 0xee) {
        return 0xeeeeee;
    }
    return ((cmd ^ 0xff) << 16) | ((uint32_t)s_frames << 8) | 0x3c;
}

static IRAM_ATTR void b_isr(void *arg)
{
    static int last_sda = 1, last_scl = 1;
    uint32_t in = REG_READ(GPIO_IN_REG);
    int sda = (in >> PIN_DATA) & 1, scl = (in >> PIN_CLK) & 1;
    if(b_phase == PHASE_I2C) {
        if(scl != last_scl) {
            if(scl) {
                i2c_scl_rising(sda);
            } else {
                i2c_scl_falling();
            }
        } else if(sda != last_sda && scl) {
            b_drive_bit(1);
            if(!sda) { // START (or repeated START)
                i_state = I_ADDR;
                i_bit = 0;
                i_byte = 0;
                i_rx = 0;
            } else { // STOP
                i_state = I_IDLE;
                if(i_switch) {
                    b_phase = PHASE_SPI;
                }
            }
        }
    } else if(b_phase == PHASE_SPI && scl != last_scl) {
        // The system timer, not the cycle counter: the counter stops while
        // this CPU sleeps in WFI between frames (two-chip setup).
        int64_t now = esp_timer_get_time();
        if(!scl && now - s_last_edge > 150) {
            s_bit = 0; // a new frame after an idle clock
            s_cmd = 0;
            b_drive_bit(1); // the last response bit was held until now
        }
        s_last_edge = now;
        if(scl) { // rising: sample
            if(s_bit < 8) {
                s_cmd = (s_cmd << 1) | sda;
            }
            s_bit++;
            // The last bit stays on the line: A samples it after this
            // edge, and this interrupt runs first.
            if(s_bit == 32 && s_switch) {
                b_phase = PHASE_CAN;
            }
        } else if(s_bit >= 8 && s_bit < 32) { // falling: the response
            if(s_bit == 8) {
                s_frames++;
                if(s_frames <= 8) {
                    b_cmds[s_frames - 1] = s_cmd;
                }
                s_switch = s_cmd == 0xee;
                s_resp = spi_response(s_cmd);
            }
            b_drive_bit((s_resp >> (31 - s_bit)) & 1);
        }
    }
    last_sda = sda;
    last_scl = scl;
}

// ---------------------------------------------------------------- role A: I2C master

static int a_stretch_us;
static void half(void) { wait_us(8 + rnd() % 23); }
static void sda(int value) { set_line(ROLE_A, DATA, !value); }
static void scl_low(void) { set_line(ROLE_A, CLK, 1); }
static void scl_release(void)
{
    set_line(ROLE_A, CLK, 0);
    uint32_t start = cycles();
    while(!read_line(CLK)) {
        if(cycles() - start > 5000 * CYCLES_PER_US) {
            break;
        }
    }
    int waited = (cycles() - start) / CYCLES_PER_US;
    if(waited > a_stretch_us) {
        a_stretch_us = waited;
    }
}
static void i2c_start(void)
{
    sda(1);
    scl_release();
    half();
    sda(0);
    half();
    scl_low();
    half();
}
static void i2c_stop(void)
{
    sda(0);
    half();
    scl_release();
    half();
    sda(1);
    half();
}
static void write_bit(int b)
{
    sda(b);
    half();
    scl_release();
    half();
    scl_low();
}
static int read_bit(void)
{
    sda(1);
    half();
    scl_release();
    int b = read_line(DATA);
    half();
    scl_low();
    return b;
}
static int write_byte(int value)
{
    for(int i = 7; i >= 0; i--) {
        write_bit((value >> i) & 1);
    }
    return read_bit() == 0;
}
static int read_byte(int ack)
{
    int value = 0;
    for(int i = 0; i < 8; i++) {
        value = (value << 1) | read_bit();
    }
    write_bit(!ack);
    return value;
}

static void a_i2c(void)
{
    uint32_t bench_start = cycles();
    static const uint8_t out[4] = { 0xa5, 0x5a, 0x0f, 0xf0 };
    uint8_t in[4];
    int acks = 0;
    i2c_start();
    int addr_ack = write_byte(0x3a << 1);
    for(int i = 0; i < 4; i++) {
        acks += write_byte(out[i]);
    }
    i2c_start(); // repeated START, then read back
    int read_ack = write_byte((0x3a << 1) | 1);
    for(int i = 0; i < 4; i++) {
        in[i] = read_byte(i < 3);
    }
    i2c_stop();
    wait_us(300);
    i2c_start();
    int nack = !write_byte(0x21 << 1); // nobody there
    i2c_stop();
    wait_us(300);
    i2c_start();
    int ctrl_ack = write_byte(0x3b << 1); // B: move on to SPI
    i2c_stop();
    bench_i2c_cycles = cycles() - bench_start;
    printf("BB A I2C addr_ack=%d data_acks=%d read_ack=%d read=%02X%02X%02X%02X nack=%d ctrl_ack=%d stretch_us=%d\n",
           addr_ack, acks, read_ack, in[0], in[1], in[2], in[3], nack, ctrl_ack, a_stretch_us);
}

// ---------------------------------------------------------------- role A: 3-wire SPI

static uint32_t spi_frame(int cmd)
{
    uint32_t response = 0;
    for(int bit = 0; bit < 32; bit++) {
        scl_low(); // mode 3: data changes on the falling edge
        if(bit < 8) {
            sda((cmd >> (7 - bit)) & 1);
        } else if(bit == 8) {
            sda(1); // turnaround: B drives the data line from here
        }
        half();
        set_line(ROLE_A, CLK, 0);
        wait_us(1);
        if(bit >= 8) {
            response = (response << 1) | read_line(DATA);
        }
        half();
    }
    wait_us(300); // an idle clock ends the frame
    return response;
}

static void a_spi(void)
{
    uint32_t bench_start = cycles();
    static const int cmds[4] = { 0x9f, 0x42, 0x5a, 0xee };
    uint32_t r[4];
    for(int i = 0; i < 4; i++) {
        r[i] = spi_frame(cmds[i]);
    }
    bench_spi_cycles = cycles() - bench_start;
    printf("BB A SPI r=%06lX,%06lX,%06lX,%06lX\n", (unsigned long)r[0], (unsigned long)r[1],
           (unsigned long)r[2], (unsigned long)r[3]);
}

// ---------------------------------------------------------------- CAN (both roles)

#define CAN_BIT_US 100
#define CAN_BIT (CAN_BIT_US * CYCLES_PER_US)
#define CAN_SAMPLE (CAN_BIT * 7 / 10)

enum { F_STUFFED, F_CRCDEL, F_ACK, F_ACKDEL, F_EOF, F_DONE };

typedef struct {
    int role;
    int transmitting;
    uint8_t tx[128];
    int lost_at, acked, bit_error, stuff_error, form_error, stuff_bits;
    int field, eof;
    int run_bit, run_len, expect_stuff;
    uint8_t rx[128];
    int rxn, total, crc_ok;
} can_node;

static int crc15(const uint8_t *bits, int n)
{
    int crc = 0;
    for(int i = 0; i < n; i++) {
        int next = bits[i] ^ ((crc >> 14) & 1);
        crc = (crc << 1) & 0x7fff;
        if(next) {
            crc ^= 0x4599;
        }
    }
    return crc;
}

static void can_init(can_node *node, int role, int id, const uint8_t *data, int len)
{
    memset(node, 0, sizeof(*node));
    node->role = role;
    node->lost_at = -1;
    node->run_bit = 1;
    node->total = 1000;
    if(!data) {
        return; // a receiver only
    }
    node->transmitting = 1;
    int n = 0;
    node->tx[n++] = 0; // SOF
    for(int i = 10; i >= 0; i--) {
        node->tx[n++] = (id >> i) & 1;
    }
    node->tx[n++] = 0; // RTR
    node->tx[n++] = 0; // IDE
    node->tx[n++] = 0; // r0
    for(int i = 3; i >= 0; i--) {
        node->tx[n++] = (len >> i) & 1;
    }
    for(int b = 0; b < len; b++) {
        for(int i = 7; i >= 0; i--) {
            node->tx[n++] = (data[b] >> i) & 1;
        }
    }
    int crc = crc15(node->tx, n);
    for(int i = 14; i >= 0; i--) {
        node->tx[n++] = (crc >> i) & 1;
    }
}

static IRAM_ATTR int can_out(can_node *node)
{
    if(node->field == F_STUFFED && node->transmitting) {
        return node->expect_stuff ? !node->run_bit : node->tx[node->rxn];
    }
    if(node->field == F_ACK && !node->transmitting && node->crc_ok) {
        return 0;
    }
    return 1;
}

static IRAM_ATTR void can_in(can_node *node, int bit, int out)
{
    switch(node->field) {
        case F_STUFFED:
            if(node->expect_stuff) {
                if(bit == node->run_bit) {
                    node->stuff_error = 1;
                    node->field = F_DONE;
                    return;
                }
                node->stuff_bits++;
                node->run_bit = bit;
                node->run_len = 1;
                node->expect_stuff = 0;
                if(node->rxn == node->total) {
                    node->field = F_CRCDEL;
                }
                return;
            }
            if(node->transmitting && bit != out) {
                if(out == 1 && node->rxn >= 1 && node->rxn <= 12) {
                    node->transmitting = 0; // lost arbitration: keep receiving
                    node->lost_at = node->rxn;
                } else {
                    node->bit_error = 1;
                    node->field = F_DONE;
                    return;
                }
            }
            node->rx[node->rxn++] = bit;
            if(bit == node->run_bit) {
                node->run_len++;
            } else {
                node->run_bit = bit;
                node->run_len = 1;
            }
            node->expect_stuff = node->run_len == 5;
            if(node->rxn == 19) {
                int dlc = (node->rx[15] << 3) | (node->rx[16] << 2) | (node->rx[17] << 1) | node->rx[18];
                node->total = 19 + 8 * (dlc > 8 ? 8 : dlc) + 15;
            }
            if(node->rxn == node->total) {
                int crc = 0;
                for(int i = 0; i < 15; i++) {
                    crc = (crc << 1) | node->rx[node->total - 15 + i];
                }
                node->crc_ok = crc == crc15(node->rx, node->total - 15);
                if(!node->expect_stuff) {
                    node->field = F_CRCDEL;
                }
            }
            break;
        case F_CRCDEL:
            node->form_error |= bit != 1;
            node->field = F_ACK;
            break;
        case F_ACK:
            if(node->transmitting) {
                node->acked = bit == 0;
            }
            node->field = F_ACKDEL;
            break;
        case F_ACKDEL:
            node->form_error |= bit != 1;
            node->field = F_EOF;
            break;
        case F_EOF:
            node->form_error |= bit != 1;
            if(++node->eof == 7) {
                node->field = F_DONE;
            }
            break;
    }
}

// Run one frame for the nodes on this chip; bit 0 (SOF) starts at `start`.
static IRAM_ATTR void can_frame(can_node **nodes, int count, uint32_t start)
{
    int out[2];
    for(int k = 0; k < 200; k++) {
        uint32_t t = start + (uint32_t)k * CAN_BIT;
        wait_until(t);
        for(int i = 0; i < count; i++) {
            out[i] = can_out(nodes[i]);
            set_line(nodes[i]->role, DATA, !out[i]);
        }
        wait_until(t + CAN_SAMPLE);
        int bit = read_line(DATA);
        int done = 1;
        for(int i = 0; i < count; i++) {
            can_in(nodes[i], bit, out[i]);
            done &= nodes[i]->field == F_DONE;
        }
        if(done) {
            break;
        }
    }
    for(int i = 0; i < count; i++) {
        set_line(nodes[i]->role, DATA, 0);
    }
}

// Bus idle: 11 recessive bits in a row.
static void can_wait_idle(void)
{
    uint32_t since = cycles();
    while(cycles() - since < 11 * CAN_BIT) {
        if(!read_line(DATA)) {
            since = cycles();
        }
    }
}

// The next SOF edge from the other chip (0 on timeout). Interrupts are off
// here, so the timeout stays well under the interrupt watchdog.
static IRAM_ATTR uint32_t can_wait_sof(void)
{
    uint32_t start = cycles();
    while(read_line(DATA)) {
        if(cycles() - start > 200000u * CYCLES_PER_US) {
            return 0;
        }
    }
    return cycles();
}

static void can_report(const char *who, const char *frame, can_node *node)
{
    int id = 0, dlc = 0;
    for(int i = 1; i <= 11; i++) {
        id = (id << 1) | node->rx[i];
    }
    for(int i = 15; i <= 18; i++) {
        dlc = (dlc << 1) | node->rx[i];
    }
    char data[20] = "";
    for(int b = 0; b < dlc && b < 8; b++) {
        int v = 0;
        for(int i = 0; i < 8; i++) {
            v = (v << 1) | node->rx[19 + 8 * b + i];
        }
        sprintf(data + 2 * b, "%02X", v);
    }
    printf("BB %s CAN %s id=%03X dlc=%d data=%s crc_ok=%d tx=%d lost_at=%d acked=%d stuff_bits=%d errors=%d/%d/%d\n",
           who, frame, id, dlc, data, node->crc_ok, node->transmitting, node->lost_at, node->acked,
           node->stuff_bits, node->bit_error, node->stuff_error, node->form_error);
}

static const uint8_t a_payload[3] = { 0xc0, 0xff, 0xee };
static const uint8_t b_payload[2] = { 0xbe, 0xef };
#define A_ID 0x2a7
#define B_ID 0x2a5 // wins arbitration on ID bit 1

static void can_both(void)
{
    can_node a, b;
    can_node *nodes[2] = { &a, &b };
    can_init(&a, ROLE_A, A_ID, a_payload, 3);
    can_init(&b, ROLE_B, B_ID, b_payload, 2);
    portENTER_CRITICAL(&line_mux);
    uint32_t bench_start = cycles();
    can_frame(nodes, 2, cycles());
    bench_can1_cycles = cycles() - bench_start;
    portEXIT_CRITICAL(&line_mux);
    can_report("A", "f1", &a);
    can_report("B", "f1", &b);
    wait_us(11 * CAN_BIT_US);
    can_init(&a, ROLE_A, A_ID, a_payload, 3); // A retries
    can_init(&b, ROLE_B, 0, NULL, 0);
    portENTER_CRITICAL(&line_mux);
    bench_start = cycles();
    can_frame(nodes, 2, cycles());
    bench_can2_cycles = cycles() - bench_start;
    portEXIT_CRITICAL(&line_mux);
    can_report("A", "f2", &a);
    can_report("B", "f2", &b);
}

static void can_a_only(void)
{
    can_node a;
    can_node *nodes[1] = { &a };
    for(int frame = 1; frame <= 2; frame++) {
        can_init(&a, ROLE_A, A_ID, a_payload, 3);
        can_wait_idle();
        portENTER_CRITICAL(&line_mux);
        can_frame(nodes, 1, cycles());
        portEXIT_CRITICAL(&line_mux);
        can_report("A", frame == 1 ? "f1" : "f2", &a);
    }
}

static void can_b_only(void)
{
    can_node b;
    can_node *nodes[1] = { &b };
    for(int frame = 1; frame <= 2; frame++) {
        if(frame == 1) {
            can_init(&b, ROLE_B, B_ID, b_payload, 2); // joins A's SOF
        } else {
            can_init(&b, ROLE_B, 0, NULL, 0);
        }
        portENTER_CRITICAL(&line_mux);
        uint32_t sof = can_wait_sof();
        if(sof) {
            can_frame(nodes, 1, sof);
        }
        portEXIT_CRITICAL(&line_mux);
        if(!sof) {
            printf("BB B CAN f%d no SOF\n", frame);
            return;
        }
        can_report("B", frame == 1 ? "f1" : "f2", &b);
    }
}

// ---------------------------------------------------------------- main

static void setup_line(int pin)
{
    // GPIO6/7 reset to their JTAG functions (MTCK/MTDO); MTDO drives the pad.
    esp_rom_gpio_pad_select_gpio(pin);
    gpio_config_t config = {
        .pin_bit_mask = 1ull << pin,
        .mode = GPIO_MODE_INPUT,
        .pull_up_en = GPIO_PULLUP_ENABLE,
    };
    gpio_config(&config);
    REG_WRITE(GPIO_OUT_W1TC_REG, 1u << pin); // drive low whenever enabled
}

static int strapped_low(int pin)
{
    gpio_config_t config = {
        .pin_bit_mask = 1ull << pin,
        .mode = GPIO_MODE_INPUT,
        .pull_up_en = GPIO_PULLUP_ENABLE,
    };
    gpio_config(&config);
    esp_rom_delay_us(50);
    return !gpio_get_level(pin);
}

void app_main(void)
{
    int only_a = strapped_low(PIN_ONLY_A), only_b = strapped_low(PIN_ONLY_B);
    int role_a = !only_b, role_b = !only_a;
    drive_high = role_a && role_b;
    setup_line(PIN_DATA);
    setup_line(PIN_CLK);
    set_line(ROLE_A, DATA, 0);
    set_line(ROLE_A, CLK, 0);
    if(role_b) {
        gpio_install_isr_service(ESP_INTR_FLAG_IRAM);
        gpio_set_intr_type(PIN_DATA, GPIO_INTR_ANYEDGE);
        gpio_set_intr_type(PIN_CLK, GPIO_INTR_ANYEDGE);
        gpio_isr_handler_add(PIN_DATA, b_isr, NULL);
        gpio_isr_handler_add(PIN_CLK, b_isr, NULL);
    }
    vTaskDelay(pdMS_TO_TICKS(300)); // the other chip boots
    printf("BB START roles=%s%s\n", role_a ? "A" : "", role_b ? "B" : "");
    if(role_a) {
        a_i2c();
        vTaskDelay(pdMS_TO_TICKS(2));
        a_spi();
        vTaskDelay(pdMS_TO_TICKS(5));
    } else {
        while(b_phase != PHASE_CAN) {
            vTaskDelay(1);
        }
    }
    if(role_b) {
        gpio_intr_disable(PIN_DATA);
        gpio_intr_disable(PIN_CLK);
        wait_us(100); // A has sampled the last SPI bit
        set_line(ROLE_B, DATA, 0);
        printf("BB B I2C wrote=%02X%02X%02X%02X writes=%d reads=%d stretches=%d\n", b_regs[0], b_regs[1],
               b_regs[2], b_regs[3], b_writes, b_reads, b_stretches);
        printf("BB B SPI cmds=%02X,%02X,%02X,%02X frames=%d\n", b_cmds[0], b_cmds[1], b_cmds[2], b_cmds[3],
               s_frames);
    }
    if(role_a && role_b) {
        can_both();
    } else if(role_a) {
        can_a_only();
    } else {
        can_b_only();
    }
    if(role_a && role_b) {
        printf("CYCLES i2c=%lu spi=%lu can1=%lu can2=%lu\n",
               (unsigned long)bench_i2c_cycles, (unsigned long)bench_spi_cycles,
               (unsigned long)bench_can1_cycles, (unsigned long)bench_can2_cycles);
    }
    printf("BB DONE\n");
    for(;;) {
        vTaskDelay(pdMS_TO_TICKS(1000));
    }
}