static bool spi_send_cmd(spi_cmd_t *p_cmd) {
    spi_status_t rc;
    int i;
    uint16_t data;
    uint8_t pcs, last = 0;

    /* Clear dummy byte */
    if (spi_is_rx_full(CONF_TEST_SPI)) spi_read(CONF_TEST_SPI, &data, &pcs);

    /* Wait until no activity on bus */
    while(!spi_is_tx_empty(CONF_TEST_SPI));

    /* Send command */
    for (i = 0; i < p_cmd->cmd_size; i ++) {
        last = (i == (p_cmd->cmd_size-1)) &&
                (p_cmd->dummy_size == 0) &&
                (p_cmd->data_size == 0);
        rc = spi_write(CONF_TEST_SPI, p_cmd->cmd[i], TEST_DF_PCS, last);

        if (p_cmd->cmd_rx) rc |= spi_read(CONF_TEST_SPI, &data, &pcs);
        if (rc != SPI_OK) return false;
    }

    /* Send dummy clocks */
    for (i = 0; i < p_cmd->dummy_size; i ++) {
        rc  = spi_write(CONF_TEST_SPI, 0xFF, TEST_DF_PCS, 0);
        if (p_cmd->cmd_rx) rc |= spi_read(CONF_TEST_SPI, &data, &pcs);
        if (rc != SPI_OK) return false;
    }
    if (p_cmd->data_size == 0) return true;

    /* Read/Write data */
    for (i = 0; i < p_cmd->data_size; i ++) {
        last = (i == (p_cmd->data_size-1));
        rc  = spi_write(CONF_TEST_SPI, p_cmd->data[i], TEST_DF_PCS, last);
        if (p_cmd->cmd_rx) rc |= spi_read(CONF_TEST_SPI, &data, &pcs);
        if (rc != SPI_OK) return false;
        if (p_cmd->cmd_rx) p_cmd->data[i] = (uint8_t)data;
    }
    return true;
}

#define SPI_CHIP_SEL                0
#define SPI_CHIP_PCS                ((~(1u << SPI_CHIP_SEL)) & 0xF)
#define SPI_CLK_POLARITY            0
#define SPI_CLK_PHASE               0
#define SPI_CLOCK                   1000000

void sst26_init(void) {
    /* SPI configuration */
    gpio_configure_pin(SPI_MISO_GPIO, SPI_MISO_FLAGS);
    gpio_configure_pin(SPI_MOSI_GPIO, SPI_MOSI_FLAGS);
    gpio_configure_pin(SPI_SPCK_GPIO, SPI_SPCK_FLAGS);
    gpio_configure_pin(SPI_NPCS0_GPIO, SPI_NPCS0_FLAGS);
    // Enable SPI Clock
    spi_enable_clock(SPI);
    // Reset SPI
    spi_reset(SPI);
    // Disable SPI
    spi_disable(SPI);
    // Set Master Mode for data flash access
    spi_set_lastxfer(SPI);
    spi_set_master_mode(SPI);
    spi_disable_mode_fault_detect(SPI);
    // Set Chip Select
    spi_set_variable_peripheral_select(SPI);
    spi_set_peripheral_chip_select_value(SPI, SPI_CHIP_SEL);
    // Set Polarity and Phase
    spi_set_clock_polarity(SPI, SPI_CHIP_SEL, SPI_CLK_POLARITY);
    spi_set_clock_phase(SPI, SPI_CHIP_SEL, SPI_CLK_PHASE);
    // Set transfer rate
    spi_set_bits_per_transfer(SPI, SPI_CHIP_SEL, SPI_CSR_BITS_8_BIT);
    spi_set_baudrate_div(SPI, SPI_CHIP_SEL, (sysclk_get_peripheral_hz() / SPI_CLOCK));
    // No delays
    spi_set_transfer_delay(SPI, SPI_CHIP_SEL, 0, 0);
    // Since it's the only slave chip... keep signal low (?)
    spi_configure_cs_behavior(SPI, SPI_CHIP_SEL, SPI_CS_KEEP_LOW);
    // Enable SPI
    spi_enable(SPI);
}