uas-ugv/components/sx127x_driver/sx127x_driver.c

#include "sx127x_driver.h"
#include "sx127x_internal.h"
#include "sx127x_registers.h"

#include <esp_log.h>
#include <string.h>

const char *SX127X_TAG = "sx127x";

typedef struct sx127x_packet {
  char * data;
  size_t data_len;
} sx127x_packet_t;

sx127x_config_t sx127x_config_default() {
  sx127x_config_t config = SX127X_CONFIG_DEFAULT;
  return config;
}

static esp_err_t sx127x_write_config(sx127x_t *hndl) {
  esp_err_t              ret;
  const sx127x_config_t *config = &hndl->config;

  ret = sx127x_sleep(hndl);
  SX127X_ERROR_CHECK(ret);

  ret = sx127x_set_frequency(hndl, config->frequency);
  SX127X_ERROR_CHECK(ret);

  ret = sx127x_write_register(hndl, REG_FIFO_TX_BASE_ADDR, 0);
  SX127X_ERROR_CHECK(ret);
  sx127x_write_register(hndl, REG_FIFO_RX_BASE_ADDR, 0);
  SX127X_ERROR_CHECK(ret);

  uint8_t reg_lna;
  ret = sx127x_read_register(hndl, REG_LNA, &reg_lna);
  SX127X_ERROR_CHECK(ret);
  reg_lna |= 0x03;  // set LNA boost
  ret = sx127x_write_register(hndl, REG_LNA, reg_lna);
  SX127X_ERROR_CHECK(ret);

  // set auto AGC
  ret = sx127x_write_register(hndl, REG_MODEM_CONFIG_3, CONFIG3_AUTO_AGC);
  SX127X_ERROR_CHECK(ret);

  ret = sx127x_set_tx_power(hndl, config->tx_power, true);
  SX127X_ERROR_CHECK(ret);

  ret = sx127x_set_spreading_factor(hndl, config->spreading_factor);
  SX127X_ERROR_CHECK(ret);

  ret = sx127x_set_sync_word(hndl, config->sync_word);
  SX127X_ERROR_CHECK(ret);

  ret = sx127x_set_crc(hndl, config->crc);
  SX127X_ERROR_CHECK(ret);

  ret = sx127x_standby(hndl);
  ESP_ERROR_CHECK(ret);

  return ESP_OK;
}

esp_err_t sx127x_init(const sx127x_config_t *config, sx127x_t **handle_ptr) {
  esp_err_t ret;
  sx127x_t *hndl = malloc(sizeof(sx127x_t));
  SX127X_CHECK(hndl != NULL, "malloc error", ESP_ERR_NO_MEM);

  hndl->task_handle = NULL;
  atomic_init(&hndl->task_state, SX127X_TASK_STOPPED);
  memcpy(&hndl->config, config, sizeof(sx127x_config_t));

  ret = gpio_set_direction(config->rst_io_num, GPIO_MODE_OUTPUT);
  SX127X_ERROR_CHECK2(ret, gpio_set_direction)

  // perform reset
  gpio_set_level(config->rst_io_num, 0);
  vTaskDelay(RESET_DELAY);
  gpio_set_level(config->rst_io_num, 1);
  vTaskDelay(RESET_DELAY);

  spi_bus_config_t bus_config = {
      .mosi_io_num     = config->mosi_io_num,
      .miso_io_num     = config->miso_io_num,
      .sclk_io_num     = config->sck_io_num,
      .quadhd_io_num   = -1,
      .quadwp_io_num   = -1,
      .max_transfer_sz = SX127X_MAX_TRANSFER,
  };

  ret = spi_bus_initialize(config->spi_host, &bus_config, SX127X_SPI_DMA_CHAN);
  SX127X_ERROR_CHECK2(ret, spi_bus_initialize)

  spi_device_interface_config_t device_config = {
      .command_bits     = 0,
      .address_bits     = 8,
      .dummy_bits       = 0,
      .mode             = 0,
      .duty_cycle_pos   = 0,
      .cs_ena_pretrans  = 0,
      .cs_ena_posttrans = 0,
      .clock_speed_hz   = SPI_CLOCK_HZ,  // 8mhz
      .input_delay_ns   = 0,
      .spics_io_num     = config->cs_io_num,
      .flags            = 0,
      .queue_size       = SX127X_SPI_QUEUE_SIZE,
      .pre_cb           = NULL,
      .post_cb          = NULL,
  };
  ret = spi_bus_add_device(config->spi_host, &device_config,
                           &hndl->device_handle);
  SX127X_ERROR_CHECK2(ret, spi_bus_add_device)

  // read version and check that it is compatible
  uint8_t version;
  ret = sx127x_read_register(hndl, REG_VERSION, &version);
  SX127X_ERROR_CHECK2(ret, sx127x_read_register);
  SX127X_CHECK(version == 0x12, "unsupported version %#x",
               ESP_ERR_INVALID_VERSION, version);

  ret = sx127x_write_config(hndl);
  SX127X_ERROR_CHECK(ret);

  *handle_ptr = hndl;
  return ESP_OK;
}

esp_err_t sx127x_free(sx127x_t *hndl) {
  esp_err_t ret;

  if (hndl->task_handle) {
    ret = sx127x_stop(hndl);
    SX127X_ERROR_CHECK(ret);
  }

  ret = spi_bus_remove_device(hndl->device_handle);
  SX127X_ERROR_CHECK2(ret, spi_bus_remove_device)

  ret = spi_bus_free(hndl->config.spi_host);
  SX127X_ERROR_CHECK2(ret, spi_bus_free)

  free(hndl);

  return ESP_OK;
}

void IRAM_ATTR sx127x_isr(void *arg) {
  sx127x_t * hndl = (sx127x_t *)arg;
  BaseType_t hpTaskWoken;
  xSemaphoreGiveFromISR(hndl->intr_semaphore, &hpTaskWoken);
  if (hpTaskWoken) {
    portYIELD_FROM_ISR();
  }
}

#define _TX_CHECK(_ret) \
  ret = (_ret);         \
  if (ret != ESP_OK) {  \
    goto error;         \
  }

static void sx127x_do_tx(sx127x_t *hndl, sx127x_packet_t *packet) {
  esp_err_t ret;
  uint8_t   op_mode, irq_flags, config_2;
  while (atomic_load(&hndl->task_state) == SX127X_TASK_RUNNING) {
    _TX_CHECK(sx127x_read_register(hndl, REG_OP_MODE, &op_mode));
    uint8_t mode = op_mode & SX127X_MODE;
    if (mode != SX127X_MODE_TX && mode != SX127X_MODE_FS_TX) {
      break;
    }
    vTaskDelay(1);  // wait for finish transmitting
  }

  _TX_CHECK(sx127x_standby(hndl));

  _TX_CHECK(sx127x_read_register(hndl, REG_IRQ_FLAGS, &irq_flags));
  if (irq_flags & IRQ_TX_DONE_MASK) {
    // clear tx done bit
    _TX_CHECK(sx127x_write_register(hndl, REG_IRQ_FLAGS, IRQ_TX_DONE_MASK));
  }

  _TX_CHECK(sx127x_read_register(hndl, REG_MODEM_CONFIG_2, &config_2));
  config_2 &= ~0x01;  // set explicit header mode TODO: implicit header?
  _TX_CHECK(sx127x_write_register(hndl, REG_MODEM_CONFIG_2, config_2));

  _TX_CHECK(sx127x_write_register(hndl, REG_FIFO_ADDR_PTR, 0));
  _TX_CHECK(sx127x_write_register(hndl, REG_PAYLOAD_LENGTH, 0));

  _TX_CHECK(sx127x_write_fifo(hndl, packet->data, packet->data_len));
  _TX_CHECK(sx127x_write_register(hndl, REG_PAYLOAD_LENGTH, packet->data_len));

  _TX_CHECK(sx127x_write_register(hndl, REG_OP_MODE,
                                  SX127X_LONG_RANGE | SX127X_MODE_FS_TX));
  vTaskDelay(pdMS_TO_TICKS(1));
  _TX_CHECK(sx127x_write_register(hndl, REG_OP_MODE,
                                  SX127X_LONG_RANGE | SX127X_MODE_TX));

  // wait for transmission to finish
  while (true) {
    _TX_CHECK(sx127x_read_register(hndl, REG_IRQ_FLAGS, &irq_flags));
    if (irq_flags & IRQ_TX_DONE_MASK) {  // if the transmission is done
      break;
    }
    vTaskDelay(1);
  }
  // clear tx done bit
  _TX_CHECK(sx127x_write_register(hndl, REG_IRQ_FLAGS, IRQ_TX_DONE_MASK));

error:
  if (ret != ESP_OK) {
    const char *error_name = esp_err_to_name(ret);
    ESP_LOGE(SX127X_TAG, "tx error: %s (%d)", error_name, ret);
  }
  // go back to rx mode
  sx127x_write_register(hndl, REG_OP_MODE,
                        SX127X_LONG_RANGE | SX127X_MODE_RX_CONT);
}

static void sx127x_do_rx(sx127x_t *hndl) {
  uint8_t            irq_flags, packet_len;
  sx127x_rx_packet_t packet;
  esp_err_t          ret;
  BaseType_t         pdRet;

  _TX_CHECK(sx127x_read_register(hndl, REG_IRQ_FLAGS, &irq_flags));
  // clear irq flags
  _TX_CHECK(sx127x_write_register(hndl, REG_IRQ_FLAGS, irq_flags));

  if (irq_flags & IRQ_PAYLOAD_CRC_ERROR_MASK) {
    ESP_LOGW(SX127X_TAG, "rx crc error");
    goto error;
  }
  if ((irq_flags & IRQ_RX_DONE_MASK) == 0) {
    ESP_LOGD(SX127X_TAG, "sx127x_do_rx called but no rx done");
    goto error;
  }

  sx127x_write_register(hndl, REG_OP_MODE,
                        SX127X_LONG_RANGE | SX127X_MODE_STDBY);

  // TODO: implicit header receive?
  _TX_CHECK(sx127x_read_register(hndl, REG_RX_NB_BYTES, &packet_len));
  _TX_CHECK(
      sx127x_write_register(hndl, REG_FIFO_ADDR_PTR, REG_FIFO_RX_CURRENT_ADDR));

  packet.data_len = packet_len;
  packet.data     = heap_caps_malloc(
      packet.data_len, MALLOC_CAP_8BIT | MALLOC_CAP_32BIT | MALLOC_CAP_DMA);
  if (!packet.data) {
    ESP_LOGE(SX127X_TAG, "malloc error");
    goto error;
  }
  _TX_CHECK(sx127x_read_fifo(hndl, packet.data, packet.data_len));

  sx127x_read_pkt_rssi(hndl, &packet.rssi);
  sx127x_read_pkt_snr(hndl, &packet.snr);

  pdRet = xQueueSend(hndl->rx_packet_queue, &packet, 0);
  if (pdRet != pdTRUE) {
    ESP_LOGE(SX127X_TAG, "rx queue full");
    free(packet.data);
    goto error;
  }

error:
  if (ret != ESP_OK) {
    const char *error_name = esp_err_to_name(ret);
    ESP_LOGE(SX127X_TAG, "rx error: %s (%d)", error_name, ret);
  }
  sx127x_write_register(hndl, REG_FIFO_ADDR_PTR, 0);
  // go back to rx mode
  sx127x_write_register(hndl, REG_OP_MODE,
                        SX127X_LONG_RANGE | SX127X_MODE_RX_CONT);
}

void sx127x_task(void *arg) {
  sx127x_t *       hndl       = (sx127x_t *)arg;
  TickType_t       delay_time = portMAX_DELAY;
  QueueSetHandle_t qSet       = xQueueCreateSet(8);
  xQueueAddToSet(hndl->intr_semaphore, qSet);
  xQueueAddToSet(hndl->tx_packet_queue, qSet);
  sx127x_packet_t packet;

  // be in rx mode by default
  sx127x_write_register(hndl, REG_OP_MODE,
                        SX127X_LONG_RANGE | SX127X_MODE_RX_CONT);

  while (atomic_load(&hndl->task_state) == SX127X_TASK_RUNNING) {
    QueueSetMemberHandle_t queue = xQueueSelectFromSet(qSet, delay_time);
    if (queue == hndl->intr_semaphore) {
      BaseType_t didRecv = xSemaphoreTake(hndl->intr_semaphore, 0);
      if (didRecv) {
        ESP_LOGV(SX127X_TAG, "recv from isr");

        sx127x_do_rx(hndl);
      }
    } else if (queue == hndl->tx_packet_queue) {
      BaseType_t didRecv = xQueueReceive(hndl->tx_packet_queue, &packet, 0);
      if (didRecv) {
        ESP_LOGV(SX127X_TAG, "tx packet: %.*s", packet.data_len, packet.data);

        sx127x_do_tx(hndl, &packet);

        free(packet.data);
      }
    }
  }
  ESP_LOGI(SX127X_TAG, "sx127x_task exiting");
  atomic_store(&hndl->task_state, SX127X_TASK_STOPPED);
  vTaskDelete(NULL);  // must delete own task
}

esp_err_t sx127x_start(sx127x_t *hndl) {
  esp_err_t ret;

  SX127X_CHECK(hndl->task_handle == NULL, "task already running",
               ESP_ERR_INVALID_STATE);

  hndl->intr_semaphore = xSemaphoreCreateBinary();
  hndl->tx_packet_queue =
      xQueueCreate(SX127X_TX_QUEUE_LEN, sizeof(sx127x_packet_t));
  hndl->rx_packet_queue =
      xQueueCreate(SX127X_RX_QUEUE_LEN, sizeof(sx127x_rx_packet_t));

  atomic_store(&hndl->task_state, SX127X_TASK_RUNNING);
  BaseType_t pdRet =
      xTaskCreate(sx127x_task, "sx127x_task", SX127X_TASK_STACK_SIZE,
                  (void *)hndl, SX127X_TASK_PRIORITY, &hndl->task_handle);
  SX127X_CHECK(pdRet == pdPASS, "failed to create task", ESP_FAIL);

  ret = sx127x_write_register(hndl, REG_DIO_MAPPING_1, 0x00);
  SX127X_ERROR_CHECK(ret);

  gpio_config_t irq_io_config;
  irq_io_config.intr_type    = GPIO_INTR_POSEDGE;
  irq_io_config.mode         = GPIO_MODE_INPUT;
  irq_io_config.pin_bit_mask = (1ULL << hndl->config.irq_io_num);
  irq_io_config.pull_down_en = 0;
  irq_io_config.pull_up_en   = 0;
  ret                        = gpio_config(&irq_io_config);
  SX127X_ERROR_CHECK2(ret, gpio_config)

  ret = gpio_install_isr_service(ESP_INTR_FLAG_LEVEL1);
  SX127X_ERROR_CHECK2(ret, gpio_install_isr_service);
  ret = gpio_isr_handler_add(hndl->config.irq_io_num, sx127x_isr, (void *)hndl);
  SX127X_ERROR_CHECK2(ret, gpio_isr_handler_add);

  return ESP_OK;
}

esp_err_t sx127x_stop(sx127x_t *hndl) {
  esp_err_t ret;

  SX127X_CHECK(hndl->task_handle != NULL, "task has not been started",
               ESP_ERR_INVALID_STATE);
  atomic_store(&hndl->task_state, SX127X_TASK_STOPPING);
  xTaskNotifyGive(hndl->task_handle);

  ret = gpio_isr_handler_remove(hndl->config.irq_io_num);
  SX127X_ERROR_CHECK2(ret, gpio_isr_handler_remove);
  gpio_uninstall_isr_service();

  while (atomic_load(&hndl->task_state) != SX127X_TASK_STOPPED) {
    vTaskDelay(10);
  }
  hndl->task_handle = NULL;

  return ESP_OK;
}

esp_err_t sx127x_send_packet(sx127x_t *hndl, const char *data, size_t data_len,
                             TickType_t ticks_to_wait) {
  SX127X_CHECK(atomic_load(&hndl->task_state) == SX127X_TASK_RUNNING,
               "task not running", ESP_ERR_INVALID_STATE);
  SX127X_CHECK(data_len < SX127_MAX_PACKET_LEN, "packet len too long: %d",
               ESP_ERR_INVALID_ARG, data_len);
  sx127x_packet_t packet;
  packet.data_len = data_len;
  packet.data     = heap_caps_malloc(
      data_len, MALLOC_CAP_8BIT | MALLOC_CAP_32BIT | MALLOC_CAP_DMA);
  SX127X_CHECK(packet.data != NULL, "malloc error", ESP_ERR_NO_MEM);
  memcpy(packet.data, data, data_len);
  BaseType_t pdRet = xQueueSend(hndl->tx_packet_queue, &packet, ticks_to_wait);
  SX127X_CHECK(pdRet == pdTRUE, "tx queue full", ESP_ERR_TIMEOUT);
  return ESP_OK;
}

esp_err_t sx127x_recv_packet(sx127x_t *hndl, sx127x_rx_packet_t *packet,
                             TickType_t ticks_to_wait) {
  SX127X_CHECK(atomic_load(&hndl->task_state) == SX127X_TASK_RUNNING,
               "task not running", ESP_ERR_INVALID_STATE);
  SX127X_CHECK(packet != NULL, "packet must not be NULL", ESP_ERR_INVALID_ARG);
  BaseType_t pdRet =
      xQueueReceive(hndl->rx_packet_queue, packet, ticks_to_wait);
  if (pdRet != pdTRUE) {
    ESP_LOGV(SX127X_TAG, "timeout on recv_packet");
    return ESP_ERR_TIMEOUT;
  }
  return ESP_OK;
}

QueueHandle_t sx127x_get_recv_queue(sx127x_t *hndl) {
  return hndl->rx_packet_queue;
}

void sx127x_packet_rx_free(sx127x_rx_packet_t *packet) { free(packet->data); }