/// Main Function
int main(void) {
  //  Read SX1262 registers 0x00 to 0x0F
  read_registers();
  return 0;
}

/// Read SX1262 registers
static void read_registers(void) {
  //  Init the SPI port
  SX126xIoInit();

  //  Read and print the first 16 registers: 0 to 15
  for (uint16_t addr = 0; addr < 0x10; addr++) {
    //  Read the register
    uint8_t val = SX126xReadRegister(addr);

    //  Print the register value
    printf("Register 0x%02x = 0x%02x\r\n", addr, val);
  }
}

/// Read an SX1262 Register at the specified address
uint8_t SX126xReadRegister(uint16_t address) {
  //  Read one register and return the value
  uint8_t data;
  SX126xReadRegisters(address, &data, 1);
  return data;
}

/// Read one or more SX1262 Registers at the specified address.
/// `size` is the number of registers to read.
void SX126xReadRegisters(uint16_t address, uint8_t *buffer, uint16_t size) {
  //  Wake up SX1262 if sleeping
  SX126xCheckDeviceReady();

  //  Read the SX1262 registers
  int rc = sx126x_read_register(NULL, address, buffer, size);
  assert(rc == 0);

  //  Wait for SX1262 to be ready
  SX126xWaitOnBusy();
}

/// Send a Read Register Command to SX1262 over SPI
/// and return the results in `buffer`. `size` is the
/// number of registers to read.
static int sx126x_read_register(const void* context, const uint16_t address, uint8_t* buffer, const uint8_t size) {
  //  Reserve 4 bytes for our SX1262 Command Buffer
  uint8_t buf[SX126X_SIZE_READ_REGISTER] = { 0 };

  //  Init the SX1262 Command Buffer
  buf[0] = RADIO_READ_REGISTER;       //  Command ID
  buf[1] = (uint8_t) (address >> 8);  //  MSB of Register ID
  buf[2] = (uint8_t) (address >> 0);  //  LSB of Register ID
  buf[3] = 0;                         //  Unused

  //  Transmit the Command Buffer over SPI 
  //  and receive the Result Buffer
  int status = sx126x_hal_read( 
    context,  //  Context (unsued)
    buf,      //  Command Buffer
    SX126X_SIZE_READ_REGISTER,  //  Command Buffer Size: 4 bytes
    buffer,   //  Result Buffer
    size,     //  Result Buffer Size
    NULL      //  Status not required
  );
  return status;
}

#define READ_REGISTERS

## Build PineDio USB Driver
make

## Run PineDio USB Driver
sudo ./lora-sx1262

Register 0x00 = 0x00
...
Register 0x08 = 0x80
Register 0x09 = 0x00
Register 0x0a = 0x01

/// TODO: We are using LoRa Frequency 923 MHz 
/// for Singapore. Change this for your region.
#define USE_BAND_923

/// LoRa Parameters
#define LORAPING_TX_OUTPUT_POWER            14        /* dBm */

#define LORAPING_BANDWIDTH                  0         /* [0: 125 kHz, */
                                                      /*  1: 250 kHz, */
                                                      /*  2: 500 kHz, */
                                                      /*  3: Reserved] */
#define LORAPING_SPREADING_FACTOR           7         /* [SF7..SF12] */
#define LORAPING_CODINGRATE                 1         /* [1: 4/5, */
                                                      /*  2: 4/6, */
                                                      /*  3: 4/7, */
                                                      /*  4: 4/8] */
#define LORAPING_PREAMBLE_LENGTH            8         /* Same for Tx and Rx */
#define LORAPING_SYMBOL_TIMEOUT             5         /* Symbols */
#define LORAPING_FIX_LENGTH_PAYLOAD_ON      false
#define LORAPING_IQ_INVERSION_ON            false

#define LORAPING_TX_TIMEOUT_MS              3000    /* ms */
#define LORAPING_RX_TIMEOUT_MS              10000    /* ms */
#define LORAPING_BUFFER_SIZE                64      /* LoRa message size */

/// Command to initialise the LoRa Driver.
/// Assume that create_task has been called to init the Event Queue.
static void init_driver(char *buf, int len, int argc, char **argv) {
  //  Set the LoRa Callback Functions
  RadioEvents_t radio_events;
  memset(&radio_events, 0, sizeof(radio_events));  //  Must init radio_events to null, because radio_events lives on stack!
  radio_events.TxDone    = on_tx_done;     //  Packet has been transmitted
  radio_events.RxDone    = on_rx_done;     //  Packet has been received
  radio_events.TxTimeout = on_tx_timeout;  //  Transmit Timeout
  radio_events.RxTimeout = on_rx_timeout;  //  Receive Timeout
  radio_events.RxError   = on_rx_error;    //  Receive Error

  //  Init the SPI Port and the LoRa Transceiver
  Radio.Init(&radio_events);

  //  Set the LoRa Frequency
  Radio.SetChannel(RF_FREQUENCY);

  //  Configure the LoRa Transceiver for transmitting messages
  Radio.SetTxConfig(
    MODEM_LORA,
    LORAPING_TX_OUTPUT_POWER,
    0,        //  Frequency deviation: Unused with LoRa
    LORAPING_BANDWIDTH,
    LORAPING_SPREADING_FACTOR,
    LORAPING_CODINGRATE,
    LORAPING_PREAMBLE_LENGTH,
    LORAPING_FIX_LENGTH_PAYLOAD_ON,
    true,     //  CRC enabled
    0,        //  Frequency hopping disabled
    0,        //  Hop period: N/A
    LORAPING_IQ_INVERSION_ON,
    LORAPING_TX_TIMEOUT_MS
  );

  //  Configure the LoRa Transceiver for receiving messages
  Radio.SetRxConfig(
    MODEM_LORA,
    LORAPING_BANDWIDTH,
    LORAPING_SPREADING_FACTOR,
    LORAPING_CODINGRATE,
    0,        //  AFC bandwidth: Unused with LoRa
    LORAPING_PREAMBLE_LENGTH,
    LORAPING_SYMBOL_TIMEOUT,
    LORAPING_FIX_LENGTH_PAYLOAD_ON,
    0,        //  Fixed payload length: N/A
    true,     //  CRC enabled
    0,        //  Frequency hopping disabled
    0,        //  Hop period: N/A
    LORAPING_IQ_INVERSION_ON,
    true      //  Continuous receive mode
  );    
}

/// Main Function
int main(void) {
  //  Init SX1262 driver
  init_driver();

  //  TODO: Do we need to wait?
  sleep(1);

  //  Send a LoRa message
  send_message();
  return 0;
}

/// Send a LoRa message. Assume that SX1262 driver has been initialised.
static void send_message(void) {
  //  Send the "PING" message
  send_once(1);
}

/// We send a "PING" message and expect a "PONG" response
const uint8_t loraping_ping_msg[] = "PING";
const uint8_t loraping_pong_msg[] = "PONG";

/// 64-byte buffer for our LoRa message
static uint8_t loraping_buffer[LORAPING_BUFFER_SIZE];

/// Send a LoRa message. If is_ping is 0, send "PONG". Otherwise send "PING".
static void send_once(int is_ping) {
  //  Copy the "PING" or "PONG" message 
  //  to the transmit buffer
  if (is_ping) {
    memcpy(loraping_buffer, loraping_ping_msg, 4);
  } else {
    memcpy(loraping_buffer, loraping_pong_msg, 4);
  }

  //  Fill up the remaining space in the 
  //  transmit buffer (64 bytes) with values 
  //  0, 1, 2, ...
  for (int i = 4; i < sizeof loraping_buffer; i++) {
    loraping_buffer[i] = i - 4;
  }

  //  We compute the message length, up to max 29 bytes.
  //  CAUTION: Anything more will cause message corruption!
  #define MAX_MESSAGE_SIZE 29
  uint8_t size = sizeof loraping_buffer > MAX_MESSAGE_SIZE
    ? MAX_MESSAGE_SIZE 
    : sizeof loraping_buffer;

  //  We send the transmit buffer, limited to 29 bytes.
  //  CAUTION: Anything more will cause message corruption!
  Radio.Send(loraping_buffer, size);

  //  TODO: Previously we send 64 bytes, which gets garbled consistently.
  //  Does CH341 limit SPI transfers to 31 bytes?
  //  (Including 2 bytes for SX1262 SPI command header)
  //  Radio.Send(loraping_buffer, sizeof loraping_buffer);
}

/// Callback Function that is called when our LoRa message has been transmitted
static void on_tx_done(void) {
  //  Log the success status
  loraping_stats.tx_success++;

  //  Switch the LoRa Transceiver to 
  //  low power, sleep mode
  Radio.Sleep();
}

/// Callback Function that is called when our LoRa message couldn't be transmitted due to timeout
static void on_tx_timeout(void) {
  //  Switch the LoRa Transceiver to 
  //  low power, sleep mode
  Radio.Sleep();

  //  Log the timeout
  loraping_stats.tx_timeout++;
}

#define SEND_MESSAGE

## Build PineDio USB Driver
make

## Run PineDio USB Driver
sudo ./lora-sx1262

send_message
RadioSend: size=29
50 49 4e 47 00 01 02 03 04 05 06 07 08 09 0a 0b 0c 0d 0e 0f 10 11 12 13 14 15 16 17 18 

LoRaP2P Rx Test
Starting Radio.Rx
OnRxDone: Timestamp=18, RssiValue=-28 dBm, SnrValue=13, 
Data=50 49 4E 47 00 01 02 03 04 05 06 07 08 09 0A 0B 0C 0D 0E 0F 10 11 12 13 14 15 16 17 18 

/// Main Function
int main(void) {
  //  TODO: Create a Background Thread 
  //  to handle LoRa Events
  create_task();

  //  Init SX1262 driver
  init_driver();

  //  TODO: Do we need to wait?
  sleep(1);

  //  Handle LoRa events for the next 10 seconds
  for (int i = 0; i < 10; i++) {
    //  Prepare to receive a LoRa message
    receive_message();

    //  Process the received LoRa message, if any
    RadioOnDioIrq(NULL);
    
    //  Sleep for 1 second
    usleep(1000 * 1000);
  }
  return 0;
}

/// Receive a LoRa message. Assume that SX1262 driver has been initialised.
/// Assume that create_task has been called to init the Event Queue.
static void receive_message(void) {
  //  Receive a LoRa message within the timeout period
  Radio.Rx(LORAPING_RX_TIMEOUT_MS);
}

/// Callback Function that is called when a LoRa message has been received
static void on_rx_done(
  uint8_t *payload,  //  Buffer containing received LoRa message
  uint16_t size,     //  Size of the LoRa message
  int16_t rssi,      //  Signal strength
  int8_t snr) {      //  Signal To Noise ratio

  //  Switch the LoRa Transceiver to low power, sleep mode
  Radio.Sleep();

  //  Log the signal strength, signal to noise ratio
  loraping_rxinfo_rxed(rssi, snr);

  //  Copy the received packet
  if (size > sizeof loraping_buffer) {
    size = sizeof loraping_buffer;
  }
  loraping_rx_size = size;
  memcpy(loraping_buffer, payload, size);

  //  Dump the contents of the received packet
  for (int i = 0; i < loraping_rx_size; i++) {
    printf("%02x ", loraping_buffer[i]);
  }
  printf("\r\n");
}

/// Callback Function that is called when no LoRa messages could be received due to timeout
static void on_rx_timeout(void) {
  //  Switch the LoRa Transceiver to low power, sleep mode
  Radio.Sleep();

  //  Log the timeout
  loraping_stats.rx_timeout++;
}

/// Callback Function that is called when we couldn't receive a LoRa message due to error
static void on_rx_error(void) {
  //  Log the error
  loraping_stats.rx_error++;

  //  Switch the LoRa Transceiver to low power, sleep mode
  Radio.Sleep();
}

#define RECEIVE_MESSAGE

## Build PineDio USB Driver
make

## Run PineDio USB Driver
sudo ./lora-sx1262

LoRap2p Tx Test
send: 48 65 6c 6c 6f 00 01 02 03 04 05 06 07 08 09 0a 0b 0c 0d 0e 0f 10 11 12 13 14 15 16 
OnTxDone

IRQ_RX_DONE
03 04 05 06 07 08 09 0a 0b 0c 0d 0e 0f 10 11 12 13 14 15 16 48 65 6c 6c 6f 00 01 02 
IRQ_PREAMBLE_DETECTED
IRQ_HEADER_VALID
receive_message

/// SPI Bus
static int spi = 0;

/// Init the SPI Bus. Return 0 on success.
static int init_spi(void) {
  //  Open the SPI Bus
  spi = open("/dev/spidev1.0", O_RDWR);
  assert(spi > 0);

  //  Set to SPI Mode 0
  uint8_t mmode = SPI_MODE_0;
  int rc = ioctl(spi, SPI_IOC_WR_MODE, &mmode);
  assert(rc == 0);

  //  Set LSB/MSB Mode
  uint8_t lsb = 0;
  rc = ioctl(spi, SPI_IOC_WR_LSB_FIRST, &lsb);
  assert(rc == 0);
  return 0;
}

/// Blocking call to transmit and receive buffers on SPI. Return 0 on success.
static int transfer_spi(const uint8_t *tx_buf, uint8_t *rx_buf, uint16_t len) {
  assert(spi > 0);
  assert(len > 0);
  assert(len <= 31);  //  CAUTION: CH341 SPI doesn't seem to support 32-byte SPI transfers 

  //  Prepare SPI Transfer
  struct spi_ioc_transfer spi_trans;
  memset(&spi_trans, 0, sizeof(spi_trans));
  spi_trans.tx_buf = (unsigned long) tx_buf;  //  Transmit Buffer
  spi_trans.rx_buf = (unsigned long) rx_buf;  //  Receive Buffer
  spi_trans.cs_change = true;   //  Set SPI Chip Select to Low
  spi_trans.len       = len;    //  How many bytes
  printf("spi tx: "); for (int i = 0; i < len; i++) { printf("%02x ", tx_buf[i]); } printf("\n");

  //  Transfer and receive the SPI buffers
  int rc = ioctl(spi, SPI_IOC_MESSAGE(1), &spi_trans);
  assert(rc >= 0);
  assert(rc == len);

  printf("spi rx: "); for (int i = 0; i < len; i++) { printf("%02x ", rx_buf[i]); } printf("\n");
  return 0;
}

//  CAUTION: CH341 SPI doesn't seem to 
//  support 32-byte SPI transfers 
assert(len <= 31);

static int sx126x_write_buffer(const void* context, const uint8_t offset, const uint8_t* buffer, const uint8_t size) {
  //  Prepare the Write Buffer Command (2 bytes)
  uint8_t buf[SX126X_SIZE_WRITE_BUFFER] = { 0 };
  buf[0] = RADIO_WRITE_BUFFER;  //  Write Buffer Command
  buf[1] = offset;              //  Write Buffer Offset

  //  Transfer the Write Buffer Command to SX1262 over SPI
  return sx126x_hal_write(
    context,  //  Context
    buf,      //  Command Buffer
    SX126X_SIZE_WRITE_BUFFER,  //  Command Buffer Size (2 bytes)
    buffer,   //  Write Data Buffer
    size      //  Write Data Buffer Size
  );
}

/**
 * Radio data transfer - write
 *
 * @remark Shall be implemented by the user
 *
 * @param [in] context          Radio implementation parameters
 * @param [in] command          Pointer to the buffer to be transmitted
 * @param [in] command_length   Buffer size to be transmitted
 * @param [in] data             Pointer to the buffer to be transmitted
 * @param [in] data_length      Buffer size to be transmitted
 *
 * @returns Operation status
 */
static int sx126x_hal_write( 
  const void* context, const uint8_t* command, const uint16_t command_length,
  const uint8_t* data, const uint16_t data_length ) {
  printf("sx126x_hal_write: command_length=%d, data_length=%d\n", command_length, data_length);

  //  Total length is command + data length
  uint16_t len = command_length + data_length;
  assert(len > 0);
  assert(len <= SPI_BUFFER_SIZE);

  //  Clear the SPI Transmit and Receive buffers
  memset(&spi_tx_buf, 0, len);
  memset(&spi_rx_buf, 0, len);

  //  Copy command bytes to SPI Transmit Buffer
  memcpy(&spi_tx_buf, command, command_length);

  //  Copy data bytes to SPI Transmit Buffer
  memcpy(&spi_tx_buf[command_length], data, data_length);

  //  Transmit and receive the SPI buffers
  int rc = transfer_spi(spi_tx_buf, spi_rx_buf, len);
  assert(rc == 0);
  return 0;
}

/// Max size of SPI transfers
#define SPI_BUFFER_SIZE 1024

/// SPI Transmit Buffer
static uint8_t spi_tx_buf[SPI_BUFFER_SIZE];

/// SPI Receive Buffer
static uint8_t spi_rx_buf[SPI_BUFFER_SIZE];

static int sx126x_read_buffer(const void* context, const uint8_t offset, uint8_t* buffer, const uint8_t size) {
  //  Prepare the Read Buffer Command (3 bytes)
  uint8_t buf[SX126X_SIZE_READ_BUFFER] = { 0 };
  buf[0] = RADIO_READ_BUFFER;  //  Read Buffer Command
  buf[1] = offset;             //  Read Buffer Offset
  buf[2] = 0;                  //  NOP

  //  Transfer the Read Buffer Command to SX1262 over SPI
  int status = sx126x_hal_read( 
    context,  //  Context
    buf,      //  Command Buffer
    SX126X_SIZE_READ_BUFFER,  //  Command Buffer Size (3 bytes)
    buffer,   //  Read Data Buffer
    size,     //  Read Data Buffer Size
    NULL      //  Ignore the status
  );
  return status;
}

/**
 * Radio data transfer - read
 *
 * @remark Shall be implemented by the user
 *
 * @param [in] context          Radio implementation parameters
 * @param [in] command          Pointer to the buffer to be transmitted
 * @param [in] command_length   Buffer size to be transmitted
 * @param [in] data             Pointer to the buffer to be received
 * @param [in] data_length      Buffer size to be received
 * @param [out] status          If not null, return the second SPI byte received as status
 *
 * @returns Operation status
 */
static int sx126x_hal_read( 
  const void* context, const uint8_t* command, const uint16_t command_length,
  uint8_t* data, const uint16_t data_length, uint8_t *status ) {
  printf("sx126x_hal_read: command_length=%d, data_length=%d\n", command_length, data_length);

  //  Total length is command + data length
  uint16_t len = command_length + data_length;
  assert(len > 0);
  assert(len <= SPI_BUFFER_SIZE);

  //  Clear the SPI Transmit and Receive buffers
  memset(&spi_tx_buf, 0, len);
  memset(&spi_rx_buf, 0, len);

  //  Copy command bytes to SPI Transmit Buffer
  memcpy(&spi_tx_buf, command, command_length);

  //  Transmit and receive the SPI buffers
  int rc = transfer_spi(spi_tx_buf, spi_rx_buf, len);
  assert(rc == 0);

  //  Copy SPI Receive buffer to data buffer
  memcpy(data, &spi_rx_buf[command_length], data_length);

  //  Return the second SPI byte received as status
  if (status != NULL) {
    assert(len >= 2);
    *status = spi_rx_buf[1];
  }
  return 0;
}

/// Blocking call to transmit and receive buffers on SPI. Return 0 on success.
static int transfer_spi(const uint8_t *tx_buf, uint8_t *rx_buf, uint16_t len) {
  //  CAUTION: CH341 SPI doesn't seem to 
  //  support 32-byte SPI transfers 
  assert(len <= 31);

sx126x_hal_write: 
  command_length=2, 
  data_length=29
spi tx: 
  0e 00 
  50 49 4e 47 00 01 02 03 04 05 06 07 08 09 0a 0b 0c 0d 0e 0f 10 11 12 13 14 15 16 17 18 
spi rx: 
  a2 a2 
  a2 a2 a2 a2 a2 a2 a2 a2 a2 a2 a2 a2 a2 a2 a2 a2 a2 a2 a2 a2 a2 a2 a2 a2 a2 a2 a2 a2 a2 

sx126x_hal_read: 
  command_length=3, 
  data_length=28
spi tx: 
  1e 00 00 
  00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 
spi rx: 
  d2 d2 d2 
  48 65 6c 6c 6f 00 01 02 03 04 05 06 07 08 09 0a 0b 0c 0d 0e 0f 10 11 12 13 14 15 16 

/// TODO: Create a Background Thread to handle LoRa Events
static void create_task(void) {
  //  Init the Event Queue
  ble_npl_eventq_init(&event_queue);

  //  Init the Event
  ble_npl_event_init(
    &event,        //  Event
    handle_event,  //  Event Handler Function
    NULL           //  Argument to be passed to Event Handler
  );

  //  TODO: Create a Background Thread to process the Event Queue
  //  nimble_port_freertos_init(task_callback);
}

## Install DKMS so that we may load Kernel Drivers dynamically
sudo pacman -Syu dkms base-devel --needed

## Install Kernel Headers for Manjaro: https://linuxconfig.org/manjaro-linux-kernel-headers-installation
uname -r 
## Should show "5.14.12-1-MANJARO-ARM" or similar
sudo pacman -S linux-headers
pacman -Q | grep headers
## Should show "linux-headers 5.14.12-1" or similar

## Reboot to be safe
sudo reboot

## Install CH341 SPI Driver
git clone https://codeberg.org/JF002/spi-ch341-usb
pushd spi-ch341-usb
## TODO: Edit Makefile and change...
##   KERNEL_DIR  = /usr/src/linux-headers-$(KVERSION)/
## To...
##   KERNEL_DIR  = /lib/modules/$(KVERSION)/build
make
sudo make install
popd

## Unload the CH341 Non-SPI Driver if it has been automatically loaded
lsmod | grep ch341
sudo rmmod ch341

## Load the CH341 SPI Driver
sudo modprobe spi-ch341-usb

## Download PineDio USB Driver
git clone --recursive https://github.com/lupyuen/lora-sx1262
cd lora-sx1262

## TODO: Edit src/main.c and uncomment 
## READ_REGISTERS, SEND_MESSAGE or RECEIVE_MESSAGE.
## See "PineDio USB Operations" below.

## TODO: Edit src/main.c and update the LoRa Parameters.
## See "LoRa Parameters" above.

## Build PineDio USB Driver
make

## Run PineDio USB Driver Demo
sudo ./lora-sx1262

void RadioInit( RadioEvents_t *events ) {
  //  We copy the Event Callbacks from "events", because
  //  "events" may be stored on the stack
  assert(events != NULL);
  memcpy(&RadioEvents, events, sizeof(RadioEvents));
  //  Previously: RadioEvents = events;

  //  Init SPI and GPIO Ports, wake up the LoRa Module,
  //  init TCXO Control and RF Switch Control.
  SX126xInit( RadioOnDioIrq );

  //  Set LoRa Module to standby mode
  SX126xSetStandby( STDBY_RC );

  //  TODO: Declare the power regulation used to power the device
  //  This command allows the user to specify if DC-DC or LDO is used for power regulation.
  //  Using only LDO implies that the Rx or Tx current is doubled

  //  #warning SX126x is set to LDO power regulator mode (instead of DC-DC)
  //  SX126xSetRegulatorMode( USE_LDO );   //  Use LDO

  //  #warning SX126x is set to DC-DC power regulator mode (instead of LDO)
  SX126xSetRegulatorMode( USE_DCDC );  //  Use DC-DC

  //  Set the base addresses of the Read and Write Buffers to 0
  SX126xSetBufferBaseAddress( 0x00, 0x00 );

  //  TODO: Set the correct transmit power and ramp up time
  SX126xSetTxParams( 22, RADIO_RAMP_3400_US );
  //  TODO: Previously: SX126xSetTxParams( 0, RADIO_RAMP_200_US );

  //  Set the DIO Interrupt Events:
  //  All LoRa Events will trigger interrupts on DIO1
  SX126xSetDioIrqParams(
    IRQ_RADIO_ALL,   //  Interrupt Mask
    IRQ_RADIO_ALL,   //  Interrupt Events for DIO1
    IRQ_RADIO_NONE,  //  Interrupt Events for DIO2
    IRQ_RADIO_NONE   //  Interrupt Events for DIO3
  );

  //  Add registers to the retention list (4 is the maximum possible number)
  RadioAddRegisterToRetentionList( REG_RX_GAIN );
  RadioAddRegisterToRetentionList( REG_TX_MODULATION );

  //  Initialize driver timeout timers
  TimerInit( &TxTimeoutTimer, RadioOnTxTimeoutIrq );
  TimerInit( &RxTimeoutTimer, RadioOnRxTimeoutIrq );

  //  Interrupt not fired yet
  IrqFired = false;
}

void RadioSetChannel( uint32_t freq ) {
  SX126xSetRfFrequency( freq );
}

void SX126xSetRfFrequency( uint32_t frequency ) {
  uint8_t buf[4];
  if( ImageCalibrated == false ) {
    SX126xCalibrateImage( frequency );
    ImageCalibrated = true;
  }
  uint32_t freqInPllSteps = SX126xConvertFreqInHzToPllStep( frequency );
  buf[0] = ( uint8_t )( ( freqInPllSteps >> 24 ) & 0xFF );
  buf[1] = ( uint8_t )( ( freqInPllSteps >> 16 ) & 0xFF );
  buf[2] = ( uint8_t )( ( freqInPllSteps >> 8 ) & 0xFF );
  buf[3] = ( uint8_t )( freqInPllSteps & 0xFF );
  SX126xWriteCommand( RADIO_SET_RFFREQUENCY, buf, 4 );
}

void RadioSetTxConfig( RadioModems_t modem, int8_t power, uint32_t fdev,
  uint32_t bandwidth, uint32_t datarate,
  uint8_t coderate, uint16_t preambleLen,
  bool fixLen, bool crcOn, bool freqHopOn,
  uint8_t hopPeriod, bool iqInverted, uint32_t timeout ) {

  //  LoRa Modulation or FSK Modulation?
  switch( modem ) {
    case MODEM_FSK:
      //  Omitted: FSK Modulation
      ...

    case MODEM_LORA:
      //  LoRa Modulation
      SX126x.ModulationParams.PacketType = 
        PACKET_TYPE_LORA;
      SX126x.ModulationParams.Params.LoRa.SpreadingFactor = 
        ( RadioLoRaSpreadingFactors_t ) datarate;
      SX126x.ModulationParams.Params.LoRa.Bandwidth =  
        Bandwidths[bandwidth];
      SX126x.ModulationParams.Params.LoRa.CodingRate = 
        ( RadioLoRaCodingRates_t )coderate;

      //  Optimise for Low Data Rate
      if( ( ( bandwidth == 0 ) && ( ( datarate == 11 ) || ( datarate == 12 ) ) ) ||
      ( ( bandwidth == 1 ) && ( datarate == 12 ) ) ) {
        SX126x.ModulationParams.Params.LoRa.LowDatarateOptimize = 0x01;
      } else {
        SX126x.ModulationParams.Params.LoRa.LowDatarateOptimize = 0x00;
      }

      //  Populate Packet Type
      SX126x.PacketParams.PacketType = PACKET_TYPE_LORA;

      //  Populate Preamble Length
      if( ( SX126x.ModulationParams.Params.LoRa.SpreadingFactor == LORA_SF5 ) ||
        ( SX126x.ModulationParams.Params.LoRa.SpreadingFactor == LORA_SF6 ) ) {
        if( preambleLen < 12 ) {
          SX126x.PacketParams.Params.LoRa.PreambleLength = 12;
        } else {
          SX126x.PacketParams.Params.LoRa.PreambleLength = preambleLen;
        }
      } else {
        SX126x.PacketParams.Params.LoRa.PreambleLength = preambleLen;
      }

      //  Populate Header Type, Payload Length, CRC Mode and Invert IQ
      SX126x.PacketParams.Params.LoRa.HeaderType = 
        ( RadioLoRaPacketLengthsMode_t )fixLen;
      SX126x.PacketParams.Params.LoRa.PayloadLength = 
        MaxPayloadLength;
      SX126x.PacketParams.Params.LoRa.CrcMode = 
        ( RadioLoRaCrcModes_t )crcOn;
      SX126x.PacketParams.Params.LoRa.InvertIQ = 
        ( RadioLoRaIQModes_t )iqInverted;

      //  Set LoRa Module to Standby Mode
      RadioStandby( );

      //  Configure LoRa Module for LoRa Modulation (or FSK Modulation)
      RadioSetModem( 
        ( SX126x.ModulationParams.PacketType == PACKET_TYPE_GFSK ) 
        ? MODEM_FSK 
        : MODEM_LORA
      );

      //  Configure Modulation Parameters
      SX126xSetModulationParams( &SX126x.ModulationParams );

      //  Configure Packet Parameters
      SX126xSetPacketParams( &SX126x.PacketParams );
      break;
  }

  // WORKAROUND - Modulation Quality with 500 kHz LoRa Bandwidth, see DS_SX1261-2_V1.2 datasheet chapter 15.1
  if( ( modem == MODEM_LORA ) && ( SX126x.ModulationParams.Params.LoRa.Bandwidth == LORA_BW_500 ) ) {
    SX126xWriteRegister( 
      REG_TX_MODULATION, 
      SX126xReadRegister( REG_TX_MODULATION ) & ~( 1 << 2 ) 
    );
  } else {
    SX126xWriteRegister( 
      REG_TX_MODULATION, 
      SX126xReadRegister( REG_TX_MODULATION ) | ( 1 << 2 ) 
    );
  }
  // WORKAROUND END

  //  Set Transmit Power
  SX126xSetRfTxPower( power );

  //  Set Transmit Timeout
  TxTimeout = timeout;
}

void SX126xSetRfTxPower( int8_t power ) {
  //  TODO: Previously: SX126xSetTxParams( power, RADIO_RAMP_40_US );
  SX126xSetTxParams( power, RADIO_RAMP_3400_US );  //  TODO
}

void RadioSetRxConfig( RadioModems_t modem, uint32_t bandwidth,
  uint32_t datarate, uint8_t coderate,
  uint32_t bandwidthAfc, uint16_t preambleLen,
  uint16_t symbTimeout, bool fixLen,
  uint8_t payloadLen,
  bool crcOn, bool freqHopOn, uint8_t hopPeriod,
  bool iqInverted, bool rxContinuous ) {

  //  Set Symbol Timeout
  RxContinuous = rxContinuous;
  if( rxContinuous == true ) {
    symbTimeout = 0;
  }

  //  Set Max Payload Length
  if( fixLen == true ) {
    MaxPayloadLength = payloadLen;
  }
  else {
    MaxPayloadLength = 0xFF;
  }

  //  LoRa Modulation or FSK Modulation?
  switch( modem )
  {
    case MODEM_FSK:
      //  Omitted: FSK Modulation
      ...

    case MODEM_LORA:
      //  LoRa Modulation
      SX126xSetStopRxTimerOnPreambleDetect( false );
      SX126x.ModulationParams.PacketType = 
        PACKET_TYPE_LORA;
      SX126x.ModulationParams.Params.LoRa.SpreadingFactor = 
        ( RadioLoRaSpreadingFactors_t )datarate;
      SX126x.ModulationParams.Params.LoRa.Bandwidth = 
        Bandwidths[bandwidth];
      SX126x.ModulationParams.Params.LoRa.CodingRate = 
        ( RadioLoRaCodingRates_t )coderate;

      //  Optimise for Low Data Rate
      if( ( ( bandwidth == 0 ) && ( ( datarate == 11 ) || ( datarate == 12 ) ) ) ||
      ( ( bandwidth == 1 ) && ( datarate == 12 ) ) ) {
        SX126x.ModulationParams.Params.LoRa.LowDatarateOptimize = 0x01;
      } else {
        SX126x.ModulationParams.Params.LoRa.LowDatarateOptimize = 0x00;
      }

      //  Populate Packet Type
      SX126x.PacketParams.PacketType = PACKET_TYPE_LORA;

      //  Populate Preamble Length
      if( ( SX126x.ModulationParams.Params.LoRa.SpreadingFactor == LORA_SF5 ) ||
          ( SX126x.ModulationParams.Params.LoRa.SpreadingFactor == LORA_SF6 ) ){
        if( preambleLen < 12 ) {
          SX126x.PacketParams.Params.LoRa.PreambleLength = 12;
        } else {
          SX126x.PacketParams.Params.LoRa.PreambleLength = preambleLen;
        }
      } else {
        SX126x.PacketParams.Params.LoRa.PreambleLength = preambleLen;
      }

      //  Populate Header Type, Payload Length, CRC Mode and Invert IQ
      SX126x.PacketParams.Params.LoRa.HeaderType = 
        ( RadioLoRaPacketLengthsMode_t )fixLen;
      SX126x.PacketParams.Params.LoRa.PayloadLength = 
        MaxPayloadLength;
      SX126x.PacketParams.Params.LoRa.CrcMode = 
        ( RadioLoRaCrcModes_t )crcOn;
      SX126x.PacketParams.Params.LoRa.InvertIQ = 
        ( RadioLoRaIQModes_t )iqInverted;

      //  Set LoRa Module to Standby Mode
      RadioStandby( );

      //  Configure LoRa Module for LoRa Modulation (or FSK Modulation)
      RadioSetModem( 
          ( SX126x.ModulationParams.PacketType == PACKET_TYPE_GFSK ) 
          ? MODEM_FSK 
          : MODEM_LORA 
      );

      //  Configure Modulation Parameters
      SX126xSetModulationParams( &SX126x.ModulationParams );

      //  Configure Packet Parameters
      SX126xSetPacketParams( &SX126x.PacketParams );

      //  Configure Symbol Timeout
      SX126xSetLoRaSymbNumTimeout( symbTimeout );

      // WORKAROUND - Optimizing the Inverted IQ Operation, see DS_SX1261-2_V1.2 datasheet chapter 15.4
      if( SX126x.PacketParams.Params.LoRa.InvertIQ == LORA_IQ_INVERTED ) {
        SX126xWriteRegister( 
          REG_IQ_POLARITY, 
          SX126xReadRegister( REG_IQ_POLARITY ) & ~( 1 << 2 ) 
        );
      } else {
        SX126xWriteRegister( 
          REG_IQ_POLARITY, 
          SX126xReadRegister( REG_IQ_POLARITY ) | ( 1 << 2 ) 
        );
      }
      // WORKAROUND END

      // Timeout Max, Timeout handled directly in SetRx function
      RxTimeout = 0xFFFF;
      break;
  }
}

void RadioSend( uint8_t *buffer, uint8_t size ) {

  //  Set the DIO Interrupt Events:
  //  Transmit Done and Transmit Timeout
  //  will trigger interrupts on DIO1
  SX126xSetDioIrqParams( 
    IRQ_TX_DONE | IRQ_RX_TX_TIMEOUT,  //  Interrupt Mask
    IRQ_TX_DONE | IRQ_RX_TX_TIMEOUT,  //  Interrupt Events for DIO1
    IRQ_RADIO_NONE,  //  Interrupt Events for DIO2
    IRQ_RADIO_NONE   //  Interrupt Events for DIO3
  );

  //  Populate the payload length
  if( SX126xGetPacketType( ) == PACKET_TYPE_LORA ) {
    SX126x.PacketParams.Params.LoRa.PayloadLength = size;
  } else {
    SX126x.PacketParams.Params.Gfsk.PayloadLength = size;
  }
  //  Configure the packet parameters
  SX126xSetPacketParams( &SX126x.PacketParams );

  //  Send message payload
  SX126xSendPayload( buffer, size, 0 );

  //  Start Transmit Timer
  TimerStart( &TxTimeoutTimer, TxTimeout );
}

///  Send message payload
void SX126xSendPayload( uint8_t *payload, uint8_t size, uint32_t timeout ) {
  //  Copy message payload to Transmit Buffer
  SX126xSetPayload( payload, size );

  //  Transmit the buffer
  SX126xSetTx( timeout );
}

/// Copy message payload to Transmit Buffer
void SX126xSetPayload( uint8_t *payload, uint8_t size ) {
  //  Copy message payload to Transmit Buffer
  SX126xWriteBuffer( 0x00, payload, size );
}

/// Copy message payload to Transmit Buffer
void SX126xWriteBuffer( uint8_t offset, uint8_t *buffer, uint8_t size ) {
  //  Wake up SX1262 if sleeping
  SX126xCheckDeviceReady( );

  //  Copy message payload to Transmit Buffer
  int rc = sx126x_write_buffer(NULL, offset, buffer, size);
  assert(rc == 0);

  //  Wait for SX1262 to be ready
  SX126xWaitOnBusy( );
}

void RadioRx( uint32_t timeout ) {

  //  Set the DIO Interrupt Events:
  //  All LoRa Events will trigger interrupts on DIO1
  SX126xSetDioIrqParams(
    IRQ_RADIO_ALL,   //  Interrupt Mask
    IRQ_RADIO_ALL,   //  Interrupt Events for DIO1
    IRQ_RADIO_NONE,  //  Interrupt Events for DIO2
    IRQ_RADIO_NONE   //  Interrupt Events for DIO3
  );

  //  Start the Receive Timer
  if( timeout != 0 ) {
    TimerStart( &RxTimeoutTimer, timeout );
  }

  if( RxContinuous == true ) {
    //  Receive continuously
    SX126xSetRx( 0xFFFFFF ); // Rx Continuous
  } else {
    //  Receive with timeout
    SX126xSetRx( RxTimeout << 6 );
  }
}

void SX126xSetRx( uint32_t timeout ) {
  uint8_t buf[3];

  //  Remember we're in Receive Mode
  SX126xSetOperatingMode( MODE_RX );

  //  Configure Receive Gain
  SX126xWriteRegister( REG_RX_GAIN, 0x94 ); // default gain

  //  Enter Receive Mode
  buf[0] = ( uint8_t )( ( timeout >> 16 ) & 0xFF );
  buf[1] = ( uint8_t )( ( timeout >> 8 ) & 0xFF );
  buf[2] = ( uint8_t )( timeout & 0xFF );
  SX126xWriteCommand( RADIO_SET_RX, buf, 3 );
}

/// Process Transmit and Receive Interrupts.
/// For BL602: Must be run in the Application
/// Task Context, not Interrupt Context because 
/// we will call printf and SPI Functions here.
void RadioIrqProcess( void ) {

  //  Remember and clear Interrupt Flag
  CRITICAL_SECTION_BEGIN( );
  const bool isIrqFired = IrqFired;
  IrqFired = false;
  CRITICAL_SECTION_END( );

  //  IrqFired must be true to process interrupts
  if( isIrqFired == true ) {
    //  Get the Interrupt Status
    uint16_t irqRegs = SX126xGetIrqStatus( );

    //  Clear the Interrupt Status
    SX126xClearIrqStatus( irqRegs );

    //  Check if DIO1 pin is High. If it is the case revert IrqFired to true
    CRITICAL_SECTION_BEGIN( );
    if( SX126xGetDio1PinState( ) == 1 ) {
      IrqFired = true;
    }
    CRITICAL_SECTION_END( );

    //  If a LoRa Message was transmitted successfully...
    if( ( irqRegs & IRQ_TX_DONE ) == IRQ_TX_DONE ) {

      //  Stop the Transmit Timer
      TimerStop( &TxTimeoutTimer );

      //!< Update operating mode state to a value lower than \ref MODE_STDBY_XOSC
      SX126xSetOperatingMode( MODE_STDBY_RC );

      //  Call the Callback Function for Transmit Done
      if( ( RadioEvents.TxDone != NULL ) ) {
        RadioEvents.TxDone( );
      }
    }

    //  If a LoRa Message was received...
    if( ( irqRegs & IRQ_RX_DONE ) == IRQ_RX_DONE ) {

      //  Stop the Receive Timer
      TimerStop( &RxTimeoutTimer );

      if( ( irqRegs & IRQ_CRC_ERROR ) == IRQ_CRC_ERROR ) {

        //  If the received message has CRC Error...
        if( RxContinuous == false ) {
          //!< Update operating mode state to a value lower than \ref MODE_STDBY_XOSC
          SX126xSetOperatingMode( MODE_STDBY_RC );
        }

        //  Call the Callback Function for Receive Error
        if( ( RadioEvents.RxError ) ) {
          RadioEvents.RxError( );
        }

      } else {
        //  If the received message has no CRC Error...
        uint8_t size;

        //  If we are receiving continously...
        if( RxContinuous == false ) {
          //!< Update operating mode state to a value lower than \ref MODE_STDBY_XOSC
          SX126xSetOperatingMode( MODE_STDBY_RC );

          // WORKAROUND - Implicit Header Mode Timeout Behavior, see DS_SX1261-2_V1.2 datasheet chapter 15.3
          SX126xWriteRegister( REG_RTC_CTRL, 0x00 );
          SX126xWriteRegister( 
            REG_EVT_CLR, 
            SX126xReadRegister( REG_EVT_CLR ) | ( 1 << 1 ) 
          );
          // WORKAROUND END
        }

        //  Copy the Received Message Payload (max 255 bytes)
        SX126xGetPayload( RadioRxPayload, &size , 255 );
        
        //  Get the Packet Status:
        //  Packet Signal Strength (RSSI), Signal-to-Noise Ratio (SNR),
        //  Signal RSSI, Frequency Error
        SX126xGetPacketStatus( &RadioPktStatus );

        //  Call the Callback Function for Receive Done
        if( ( RadioEvents.RxDone != NULL ) ) {
          RadioEvents.RxDone( 
            RadioRxPayload, 
            size, 
            RadioPktStatus.Params.LoRa.RssiPkt, 
            RadioPktStatus.Params.LoRa.SnrPkt 
          );
        }
      }
    }

/// Copy message payload from Receive Buffer
uint8_t SX126xGetPayload( uint8_t *buffer, uint8_t *size,  uint8_t maxSize ) {
  uint8_t offset = 0;

  //  Get the size and offset of the received message
  //  in the Receive Buffer
  SX126xGetRxBufferStatus( size, &offset );
  if( *size > maxSize ) {
    return 1;
  }

  //  Copy message payload from Receive Buffer
  SX126xReadBuffer( offset, buffer, *size );
  return 0;
}

/// Copy message payload from Receive Buffer
void SX126xReadBuffer( uint8_t offset, uint8_t *buffer, uint8_t size ) {
  //  Wake up SX1262 if sleeping
  SX126xCheckDeviceReady( );

  //  Copy message payload from Receive Buffer
  int rc = sx126x_read_buffer(NULL, offset, buffer, size);
  assert(rc == 0);

  //  Wait for SX1262 to be ready
  SX126xWaitOnBusy( );
}

    //  If Channel Activity Detection is complete...
    if( ( irqRegs & IRQ_CAD_DONE ) == IRQ_CAD_DONE ) {

      //!< Update operating mode state to a value lower than \ref MODE_STDBY_XOSC
      SX126xSetOperatingMode( MODE_STDBY_RC );

      //  Call Callback Function for CAD Done
      if( ( RadioEvents.CadDone != NULL ) ) {
        RadioEvents.CadDone( ( 
            ( irqRegs & IRQ_CAD_ACTIVITY_DETECTED ) 
            == IRQ_CAD_ACTIVITY_DETECTED 
        ) );
      }
    }

    //  If a LoRa Message failed to Transmit or Receive due to Timeout...
    if( ( irqRegs & IRQ_RX_TX_TIMEOUT ) == IRQ_RX_TX_TIMEOUT ) {

      //  If the message failed to Transmit due to Timeout...
      if( SX126xGetOperatingMode( ) == MODE_TX ) {

        //  Stop the Transmit Timer
        TimerStop( &TxTimeoutTimer );

        //!< Update operating mode state to a value lower than \ref MODE_STDBY_XOSC
        SX126xSetOperatingMode( MODE_STDBY_RC );

        //  Call the Callback Function for Transmit Timeout
        if( ( RadioEvents.TxTimeout != NULL ) ) {
          RadioEvents.TxTimeout( );
        }
      }

      //  If the message failed to Receive due to Timeout...
      else if( SX126xGetOperatingMode( ) == MODE_RX ) {

        //  Stop the Receive Timer
        TimerStop( &RxTimeoutTimer );

        //!< Update operating mode state to a value lower than \ref MODE_STDBY_XOSC
        SX126xSetOperatingMode( MODE_STDBY_RC );

        //  Call the Callback Function for Receive Timeout
        if( ( RadioEvents.RxTimeout != NULL ) ) {
          RadioEvents.RxTimeout( );
        }
      }
    }

    //  If LoRa Preamble was detected...
    if( ( irqRegs & IRQ_PREAMBLE_DETECTED ) == IRQ_PREAMBLE_DETECTED ) {
      //__NOP( );
    }

    //  If a valid Sync Word was detected...
    if( ( irqRegs & IRQ_SYNCWORD_VALID ) == IRQ_SYNCWORD_VALID ) {
      //__NOP( );
    }

//  Syncword for Private LoRa networks
#define LORA_MAC_PRIVATE_SYNCWORD                   0x1424

//  Syncword for Public LoRa networks (LoRaWAN)
#define LORA_MAC_PUBLIC_SYNCWORD                    0x3444

    //  If a valid Header was received...
    if( ( irqRegs & IRQ_HEADER_VALID ) == IRQ_HEADER_VALID ) {
      //__NOP( );
    }

    //  If a Header with CRC Error was received...
    if( ( irqRegs & IRQ_HEADER_ERROR ) == IRQ_HEADER_ERROR ) {

      //  Stop the Receive Timer
      TimerStop( &RxTimeoutTimer );

      if( RxContinuous == false ) {
        //!< Update operating mode state to a value lower than \ref MODE_STDBY_XOSC
        SX126xSetOperatingMode( MODE_STDBY_RC );
      }

      //  Call the Callback Function for Receive Timeout
      if( ( RadioEvents.RxTimeout != NULL ) ) {
        RadioEvents.RxTimeout( );
      }
    }
  }
}

/// Callback Function for Transmit and Receive Interrupts.
/// For BL602: This function runs in the context of the 
/// Background Application Task. So we are safe to call 
/// printf and SPI Functions now.
void RadioOnDioIrq( struct ble_npl_event *ev ) {
  //  Set the Interrupt Flag
  IrqFired = true;

  //  BL602 Note: It's OK to process the interrupt here because we are in
  //  Application Task Context, not Interrupt Context.
  //  The Reference Implementation processes the interrupt in the main loop.
  RadioIrqProcess();
}

/// Switch to Sleep Mode
void RadioSleep( void ) {
  SleepParams_t params = { 0 };
  params.Fields.WarmStart = 1;

  //  Switch to Sleep Mode and wait 2 milliseconds
  SX126xSetSleep( params );
  DelayMs( 2 );
}

/// Switch to Sleep Mode
void SX126xSetSleep( SleepParams_t sleepConfig ) {
  //  Switch off antenna (not used)
  SX126xAntSwOff( );

  //  Compute Sleep Parameter
  uint8_t value = ( 
      ( ( uint8_t )sleepConfig.Fields.WarmStart << 2 ) |
      ( ( uint8_t )sleepConfig.Fields.Reset << 1 ) |
      ( ( uint8_t )sleepConfig.Fields.WakeUpRTC ) 
  );

  if( sleepConfig.Fields.WarmStart == 0 ) {
    // Force image calibration
    ImageCalibrated = false;
  }

  //  Run Sleep Command
  SX126xWriteCommand( RADIO_SET_SLEEP, &value, 1 );
  SX126xSetOperatingMode( MODE_SLEEP );
}