makes GPIO_PIN_RST optional for the sx1276
[ExpressLRS.git] / src / lib / SX127xDriver / SX127xHal.cpp
blob4d01c97f66db52b197193c1048fd4f145423edec
1 #ifndef UNIT_TEST
3 #include "SX127xHal.h"
4 #include "logging.h"
6 SX127xHal *SX127xHal::instance = NULL;
8 SX127xHal::SX127xHal()
10 instance = this;
13 void SX127xHal::end()
15 RXenable(); // make sure the TX amp pin is disabled
16 detachInterrupt(GPIO_PIN_DIO0);
17 SPI.end();
18 IsrCallback = nullptr; // remove callbacks
21 void SX127xHal::init()
23 DBGLN("Hal Init");
25 #if defined(GPIO_PIN_PA_ENABLE) && (GPIO_PIN_PA_ENABLE != UNDEF_PIN)
26 DBGLN("Use PA ctrl pin: %d", GPIO_PIN_PA_ENABLE);
27 pinMode(GPIO_PIN_PA_ENABLE, OUTPUT);
28 digitalWrite(GPIO_PIN_PA_ENABLE, LOW);
29 #endif
31 #if defined(GPIO_PIN_TX_ENABLE) && (GPIO_PIN_TX_ENABLE != UNDEF_PIN)
32 DBGLN("Use TX pin: %d", GPIO_PIN_TX_ENABLE);
33 pinMode(GPIO_PIN_TX_ENABLE, OUTPUT);
34 digitalWrite(GPIO_PIN_TX_ENABLE, LOW);
35 #endif
37 #if defined(GPIO_PIN_RX_ENABLE) && (GPIO_PIN_RX_ENABLE != UNDEF_PIN)
38 DBGLN("Use RX pin: %d", GPIO_PIN_RX_ENABLE);
39 pinMode(GPIO_PIN_RX_ENABLE, OUTPUT);
40 digitalWrite(GPIO_PIN_RX_ENABLE, LOW);
41 #endif
43 #ifdef PLATFORM_ESP32
44 SPI.begin(GPIO_PIN_SCK, GPIO_PIN_MISO, GPIO_PIN_MOSI); // sck, miso, mosi, ss (ss can be any GPIO)
45 SPI.setBitOrder(MSBFIRST);
46 SPI.setDataMode(SPI_MODE0);
47 SPI.setFrequency(10000000);
48 #endif
50 #ifdef PLATFORM_ESP8266
51 SPI.begin();
52 SPI.setBitOrder(MSBFIRST);
53 SPI.setDataMode(SPI_MODE0);
54 SPI.setFrequency(10000000);
55 #endif
57 #ifdef PLATFORM_STM32
58 SPI.setMOSI(GPIO_PIN_MOSI);
59 SPI.setMISO(GPIO_PIN_MISO);
60 SPI.setSCLK(GPIO_PIN_SCK);
61 SPI.setBitOrder(MSBFIRST);
62 SPI.setDataMode(SPI_MODE0);
63 SPI.setClockDivider(SPI_CLOCK_DIV4); // 72 / 8 = 9 MHz
64 SPI.begin(); // SPI.setFrequency(10000000);
65 #endif
67 pinMode(GPIO_PIN_NSS, OUTPUT);
68 pinMode(GPIO_PIN_DIO0, INPUT);
70 digitalWrite(GPIO_PIN_NSS, HIGH);
72 #if defined(GPIO_PIN_RST)
73 pinMode(GPIO_PIN_RST, OUTPUT);
75 delay(100);
76 digitalWrite(GPIO_PIN_RST, 0);
77 delay(100);
78 pinMode(GPIO_PIN_RST, INPUT); // leave floating
79 #endif
81 attachInterrupt(digitalPinToInterrupt(GPIO_PIN_DIO0), dioISR, RISING);
84 uint8_t ICACHE_RAM_ATTR SX127xHal::getRegValue(uint8_t reg, uint8_t msb, uint8_t lsb)
86 if ((msb > 7) || (lsb > 7) || (lsb > msb))
88 return (ERR_INVALID_BIT_RANGE);
90 uint8_t rawValue = readRegister(reg);
91 uint8_t maskedValue = rawValue & ((0b11111111 << lsb) & (0b11111111 >> (7 - msb)));
92 return (maskedValue);
95 void ICACHE_RAM_ATTR SX127xHal::readRegisterBurst(uint8_t reg, uint8_t numBytes, uint8_t *inBytes)
97 WORD_ALIGNED_ATTR uint8_t buf[numBytes + 1];
98 buf[0] = reg | SPI_READ;
100 digitalWrite(GPIO_PIN_NSS, LOW);
101 SPI.transfer(buf, numBytes + 1);
102 digitalWrite(GPIO_PIN_NSS, HIGH);
104 memcpy(inBytes, buf + 1, numBytes);
107 uint8_t ICACHE_RAM_ATTR SX127xHal::readRegister(uint8_t reg)
109 WORD_ALIGNED_ATTR uint8_t buf[2];
111 buf[0] = reg | SPI_READ;
113 digitalWrite(GPIO_PIN_NSS, LOW);
114 SPI.transfer(buf, 2);
115 digitalWrite(GPIO_PIN_NSS, HIGH);
117 return (buf[1]);
120 uint8_t ICACHE_RAM_ATTR SX127xHal::setRegValue(uint8_t reg, uint8_t value, uint8_t msb, uint8_t lsb)
122 if ((msb > 7) || (lsb > 7) || (lsb > msb))
124 return (ERR_INVALID_BIT_RANGE);
127 uint8_t currentValue = readRegister(reg);
128 uint8_t mask = ~((0b11111111 << (msb + 1)) | (0b11111111 >> (8 - lsb)));
129 uint8_t newValue = (currentValue & ~mask) | (value & mask);
130 writeRegister(reg, newValue);
131 return (ERR_NONE);
134 void ICACHE_RAM_ATTR SX127xHal::writeRegisterFIFO(volatile uint8_t *data, uint8_t numBytes)
136 WORD_ALIGNED_ATTR uint8_t buf[numBytes + 1];
137 buf[0] = (SX127X_REG_FIFO | SPI_WRITE);
139 for (int i = 0; i < numBytes; i++) // todo check if this is the right want to handle volatiles
141 buf[i + 1] = data[i];
144 digitalWrite(GPIO_PIN_NSS, LOW);
145 #ifdef PLATFORM_STM32
146 SPI.transfer(buf, numBytes + 1);
147 #else
148 SPI.writeBytes(buf, numBytes + 1);
149 #endif
150 digitalWrite(GPIO_PIN_NSS, HIGH);
153 void ICACHE_RAM_ATTR SX127xHal::readRegisterFIFO(volatile uint8_t *data, uint8_t numBytes)
155 WORD_ALIGNED_ATTR uint8_t buf[numBytes + 1];
156 buf[0] = SX127X_REG_FIFO | SPI_READ;
158 digitalWrite(GPIO_PIN_NSS, LOW);
159 SPI.transfer(buf, numBytes + 1);
160 digitalWrite(GPIO_PIN_NSS, HIGH);
162 for (int i = 0; i < numBytes; i++) // todo check if this is the right want to handle volatiles
164 data[i] = buf[i + 1];
168 void ICACHE_RAM_ATTR SX127xHal::writeRegisterBurst(uint8_t reg, uint8_t *data, uint8_t numBytes)
170 WORD_ALIGNED_ATTR uint8_t buf[numBytes + 1];
171 buf[0] = reg | SPI_WRITE;
172 memcpy(buf + 1, data, numBytes);
174 digitalWrite(GPIO_PIN_NSS, LOW);
175 #ifdef PLATFORM_STM32
176 SPI.transfer(buf, numBytes + 1);
177 #else
178 SPI.writeBytes(buf, numBytes + 1);
179 #endif
180 digitalWrite(GPIO_PIN_NSS, HIGH);
183 void ICACHE_RAM_ATTR SX127xHal::writeRegister(uint8_t reg, uint8_t data)
185 WORD_ALIGNED_ATTR uint8_t buf[2];
187 buf[0] = reg | SPI_WRITE;
188 buf[1] = data;
190 digitalWrite(GPIO_PIN_NSS, LOW);
191 #ifdef PLATFORM_STM32
192 SPI.transfer(buf, 2);
193 #else
194 SPI.writeBytes(buf, 2);
195 #endif
196 digitalWrite(GPIO_PIN_NSS, HIGH);
199 void ICACHE_RAM_ATTR SX127xHal::TXenable()
201 #if defined(GPIO_PIN_RX_ENABLE) && (GPIO_PIN_RX_ENABLE != UNDEF_PIN)
202 digitalWrite(GPIO_PIN_RX_ENABLE, LOW);
203 #endif
204 #if defined(GPIO_PIN_TX_ENABLE) && (GPIO_PIN_TX_ENABLE != UNDEF_PIN)
205 digitalWrite(GPIO_PIN_TX_ENABLE, HIGH);
206 #endif
207 #if defined(GPIO_PIN_PA_ENABLE) && (GPIO_PIN_PA_ENABLE != UNDEF_PIN)
208 digitalWrite(GPIO_PIN_PA_ENABLE, HIGH);
209 #endif
212 void ICACHE_RAM_ATTR SX127xHal::RXenable()
214 #if defined(GPIO_PIN_RX_ENABLE) && (GPIO_PIN_RX_ENABLE != UNDEF_PIN)
215 digitalWrite(GPIO_PIN_RX_ENABLE, HIGH);
216 #endif
217 #if defined(GPIO_PIN_TX_ENABLE) && (GPIO_PIN_TX_ENABLE != UNDEF_PIN)
218 digitalWrite(GPIO_PIN_TX_ENABLE, LOW);
219 #endif
220 #if defined(GPIO_PIN_PA_ENABLE) && (GPIO_PIN_PA_ENABLE != UNDEF_PIN)
221 digitalWrite(GPIO_PIN_PA_ENABLE, HIGH);
222 #endif
225 void ICACHE_RAM_ATTR SX127xHal::TXRXdisable()
227 #if defined(GPIO_PIN_RX_ENABLE) && (GPIO_PIN_RX_ENABLE != UNDEF_PIN)
228 digitalWrite(GPIO_PIN_RX_ENABLE, LOW);
229 #endif
230 #if defined(GPIO_PIN_TX_ENABLE) && (GPIO_PIN_TX_ENABLE != UNDEF_PIN)
231 digitalWrite(GPIO_PIN_TX_ENABLE, LOW);
232 #endif
233 #if defined(GPIO_PIN_PA_ENABLE) && (GPIO_PIN_PA_ENABLE != UNDEF_PIN)
234 digitalWrite(GPIO_PIN_PA_ENABLE, LOW);
235 #endif
238 void ICACHE_RAM_ATTR SX127xHal::dioISR()
240 if (instance->IsrCallback)
241 instance->IsrCallback();
244 #endif // UNIT_TEST