Commit 3e8147a1 authored by TMRh20's avatar TMRh20

Merge remote-tracking branch 'refs/remotes/origin/Interrupts'

parents bf06db96 d37d0304
......@@ -43,13 +43,9 @@ void RF24::csn(bool mode)
_SPI.setDataMode(SPI_MODE0);
_SPI.setClockDivider(SPI_CLOCK_DIV2);
#endif
#elif defined (RF24_RPi)
_SPI.setBitOrder(RF24_BIT_ORDER);
_SPI.setDataMode(RF24_DATA_MODE);
_SPI.setClockDivider(spi_speed ? spi_speed : RF24_CLOCK_DIVIDER);
if(!mode)
_SPI.chipSelect(csn_pin);
delayMicroseconds(5);
#endif
#if !defined (RF24_LINUX)
......@@ -71,7 +67,7 @@ void RF24::ce(bool level)
inline void RF24::beginTransaction() {
#if defined (RF24_SPI_TRANSACTIONS)
_SPI.beginTransaction(SPISettings(RF_SPI_SPEED, MSBFIRST, SPI_MODE0));
_SPI.beginTransaction(SPISettings(RF24_SPI_SPEED, MSBFIRST, SPI_MODE0));
#endif
csn(LOW);
}
......@@ -92,7 +88,7 @@ uint8_t RF24::read_register(uint8_t reg, uint8_t* buf, uint8_t len)
uint8_t status;
#if defined (RF24_LINUX)
csn(LOW); //In this case, calling csn(LOW) configures the spi settings for RPi
beginTransaction(); //configures the spi settings for RPi, locks mutex and setting csn low
uint8_t * prx = spi_rxbuff;
uint8_t * ptx = spi_txbuff;
uint8_t size = len + 1; // Add register value to transmit buffer
......@@ -107,6 +103,7 @@ uint8_t RF24::read_register(uint8_t reg, uint8_t* buf, uint8_t len)
// decrement before to skip status byte
while ( --size ){ *buf++ = *prx++; }
endTransaction(); //unlocks mutex and setting csn high
#else
......@@ -130,7 +127,7 @@ uint8_t RF24::read_register(uint8_t reg)
#if defined (RF24_LINUX)
csn(LOW);
beginTransaction();
uint8_t * prx = spi_rxbuff;
uint8_t * ptx = spi_txbuff;
......@@ -140,6 +137,7 @@ uint8_t RF24::read_register(uint8_t reg)
_SPI.transfernb( (char *) spi_txbuff, (char *) spi_rxbuff, 2);
result = *++prx; // result is 2nd byte of receive buffer
endTransaction();
#else
beginTransaction();
......@@ -159,8 +157,7 @@ uint8_t RF24::write_register(uint8_t reg, const uint8_t* buf, uint8_t len)
uint8_t status;
#if defined (RF24_LINUX)
csn(LOW);
beginTransaction();
uint8_t * prx = spi_rxbuff;
uint8_t * ptx = spi_txbuff;
uint8_t size = len + 1; // Add register value to transmit buffer
......@@ -171,7 +168,7 @@ uint8_t RF24::write_register(uint8_t reg, const uint8_t* buf, uint8_t len)
_SPI.transfernb( (char *) spi_txbuff, (char *) spi_rxbuff, size);
status = *prx; // status is 1st byte of receive buffer
endTransaction();
#else
beginTransaction();
......@@ -194,7 +191,7 @@ uint8_t RF24::write_register(uint8_t reg, uint8_t value)
IF_SERIAL_DEBUG(printf_P(PSTR("write_register(%02x,%02x)\r\n"),reg,value));
#if defined (RF24_LINUX)
csn(LOW);
beginTransaction();
uint8_t * prx = spi_rxbuff;
uint8_t * ptx = spi_txbuff;
*ptx++ = ( W_REGISTER | ( REGISTER_MASK & reg ) );
......@@ -202,7 +199,7 @@ uint8_t RF24::write_register(uint8_t reg, uint8_t value)
_SPI.transfernb( (char *) spi_txbuff, (char *) spi_rxbuff, 2);
status = *prx++; // status is 1st byte of receive buffer
endTransaction();
#else
beginTransaction();
......@@ -229,7 +226,7 @@ uint8_t RF24::write_payload(const void* buf, uint8_t data_len, const uint8_t wri
IF_SERIAL_DEBUG( printf("[Writing %u bytes %u blanks]\n",data_len,blank_len); );
#if defined (RF24_LINUX)
csn(LOW);
beginTransaction();
uint8_t * prx = spi_rxbuff;
uint8_t * ptx = spi_txbuff;
uint8_t size;
......@@ -243,7 +240,7 @@ uint8_t RF24::write_payload(const void* buf, uint8_t data_len, const uint8_t wri
_SPI.transfernb( (char *) spi_txbuff, (char *) spi_rxbuff, size);
status = *prx; // status is 1st byte of receive buffer
endTransaction();
#else
......@@ -277,7 +274,7 @@ uint8_t RF24::read_payload(void* buf, uint8_t data_len)
IF_SERIAL_DEBUG( printf("[Reading %u bytes %u blanks]\n",data_len,blank_len); );
#if defined (RF24_LINUX)
csn(LOW);
beginTransaction();
uint8_t * prx = spi_rxbuff;
uint8_t * ptx = spi_txbuff;
uint8_t size;
......@@ -297,7 +294,7 @@ uint8_t RF24::read_payload(void* buf, uint8_t data_len)
*current++ = *prx++;
*current = *prx;
endTransaction();
#else
beginTransaction();
......@@ -334,18 +331,11 @@ uint8_t RF24::flush_tx(void)
uint8_t RF24::spiTrans(uint8_t cmd){
uint8_t status;
#if defined (RF24_LINUX)
csn(LOW);
status = _SPI.transfer( cmd );
#else
beginTransaction();
status = _SPI.transfer( cmd );
endTransaction();
#endif
return status;
}
......@@ -444,7 +434,7 @@ RF24::RF24(uint8_t _cepin, uint8_t _cspin, uint32_t _spi_speed):
void RF24::setChannel(uint8_t channel)
{
const uint8_t max_channel = 127;
const uint8_t max_channel = 125;
write_register(RF_CH,rf24_min(channel,max_channel));
}
......@@ -1038,10 +1028,10 @@ uint8_t RF24::getDynamicPayloadSize(void)
#if defined (RF24_LINUX)
spi_txbuff[0] = R_RX_PL_WID;
spi_rxbuff[1] = 0xff;
csn(LOW);
beginTransaction();
_SPI.transfernb( (char *) spi_txbuff, (char *) spi_rxbuff, 2);
result = spi_rxbuff[1];
csn(HIGH);
endTransaction();
#else
beginTransaction();
_SPI.transfer( R_RX_PL_WID );
......@@ -1222,19 +1212,10 @@ void RF24::closeReadingPipe( uint8_t pipe )
void RF24::toggle_features(void)
{
#if defined (RF24_LINUX)
csn(LOW);
_SPI.transfer( ACTIVATE );
_SPI.transfer( 0x73 );
csn(HIGH);
#else
beginTransaction();
_SPI.transfer( ACTIVATE );
_SPI.transfer( 0x73 );
endTransaction();
#endif
}
/****************************************************************************/
......@@ -1302,7 +1283,7 @@ void RF24::writeAckPayload(uint8_t pipe, const void* buf, uint8_t len)
uint8_t data_len = rf24_min(len,32);
#if defined (RF24_LINUX)
csn(LOW);
beginTransaction();
uint8_t * ptx = spi_txbuff;
uint8_t size = data_len + 1 ; // Add register value to transmit buffer
*ptx++ = W_ACK_PAYLOAD | ( pipe & 0b111 );
......@@ -1311,7 +1292,7 @@ void RF24::writeAckPayload(uint8_t pipe, const void* buf, uint8_t len)
}
_SPI.transfern( (char *) spi_txbuff, size);
csn(HIGH);
endTransaction();
#else
beginTransaction();
_SPI.transfer(W_ACK_PAYLOAD | ( pipe & 0b111 ) );
......
......@@ -68,8 +68,8 @@ private:
uint8_t ce_pin; /**< "Chip Enable" pin, activates the RX or TX role */
uint8_t csn_pin; /**< SPI Chip select */
#if defined (RF24_LINUX)
uint16_t spi_speed; /**< SPI Bus Speed */
#if defined (RF24_LINUX)
uint8_t spi_rxbuff[32+1] ; //SPI receive buffer (payload max 32 bytes)
uint8_t spi_txbuff[32+1] ; //SPI transmit buffer (payload max 32 bytes + 1 byte for the command)
#endif
......@@ -708,7 +708,7 @@ s *
/**
* Set RF communication channel
*
* @param channel Which RF channel to communicate on, 0-127
* @param channel Which RF channel to communicate on, 0-125
*/
void setChannel(uint8_t channel);
......
......@@ -58,7 +58,7 @@
#include <Arduino.h>
// RF modules support 10 Mhz SPI bus speed
const uint32_t RF_SPI_SPEED = 10000000;
const uint32_t RF24_SPI_SPEED = 10000000;
#if defined (ARDUINO) && !defined (__arm__) && !defined (__ARDUINO_X86__)
#if defined SPI_UART
......
......@@ -34,7 +34,7 @@ RF24 radio(7,8);
// Channel info
//
const uint8_t num_channels = 128;
const uint8_t num_channels = 126;
uint8_t values[num_channels];
//
......
......@@ -48,8 +48,7 @@ RF24 radio(RPI_V2_GPIO_P1_15, RPI_V2_GPIO_P1_24, BCM2835_SPI_SPEED_8MHZ);
//
// Channel info
//
//const uint8_t num_channels = 128;
const uint8_t num_channels = 120;
const uint8_t num_channels = 126;
uint8_t values[num_channels];
......
......@@ -14,10 +14,10 @@
#include "bcm2835.h"
#include "spi.h"
#define _SPI spi
#define RF24_BIT_ORDER BCM2835_SPI_BIT_ORDER_MSBFIRST
#define RF24_DATA_MODE BCM2835_SPI_MODE0
#define RF24_CLOCK_DIVIDER BCM2835_SPI_SPEED_8MHZ
#if defined SPI_HAS_TRANSACTION && !defined SPI_UART && !defined SOFTSPI
#define RF24_SPI_TRANSACTIONS
#endif
// GCC a Arduino Missing
#define _BV(x) (1<<(x))
#define pgm_read_word(p) (*(p))
......
......@@ -24,11 +24,12 @@ see <http://www.gnu.org/licenses/>
#define delay(x) bcm2835_delay(x)
static pthread_mutex_t pinMutex ;
static pthread_mutex_t spiMutex ;
static volatile int pinPass = -1 ;
pthread_t threadId [64];
// sysFds:
// Map a file descriptor from the /sys/class/gpio/gpioX/value
static int sysFds [64] =
{
-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1,
......@@ -97,6 +98,7 @@ void *interruptHandler (void *arg)
pthread_mutex_lock (&pinMutex) ;
isrFunctions [myPin] () ;
pthread_mutex_unlock (&pinMutex) ;
pthread_testcancel(); //Cancel at this point if we have an cancellation request.
}
return NULL ;
......@@ -105,7 +107,6 @@ void *interruptHandler (void *arg)
int attachInterrupt (int pin, int mode, void (*function)(void))
{
pthread_t threadId ;
const char *modeS ;
char fName [64] ;
char pinS [8] ;
......@@ -151,7 +152,7 @@ int attachInterrupt (int pin, int mode, void (*function)(void))
if (sysFds [bcmGpioPin] == -1)
{
sprintf (fName, "/sys/class/gpio/gpio%d/value",bcmGpioPin) ; //WRONG
sprintf (fName, "/sys/class/gpio/gpio%d/value",bcmGpioPin);
if ((sysFds [bcmGpioPin] = open (fName, O_RDWR)) < 0)
return printf ("wiringPiISR: unable to open %s: %s\n", fName, strerror (errno)) ;
}
......@@ -164,7 +165,7 @@ int attachInterrupt (int pin, int mode, void (*function)(void))
pthread_mutex_lock (&pinMutex) ;
pinPass = pin ;
pthread_create (&threadId, NULL, interruptHandler, NULL) ;
pthread_create (&threadId[bcmGpioPin], NULL, interruptHandler, NULL) ;
while (pinPass != -1)
delay (1) ;
pthread_mutex_unlock (&pinMutex) ;
......@@ -172,6 +173,50 @@ int attachInterrupt (int pin, int mode, void (*function)(void))
return 0 ;
}
int detachInterrupt (int pin)
{
char pinS [8];
const char *modeS = "none";
pid_t pid ;
if (!pthread_cancel(threadId[pin])) //Cancel the thread
{
return 0;
}
if (!close(sysFds[pin])) //Close filehandle
{
return 0;
}
/* Set wiringPi to 'none' interrupt mode */
sprintf (pinS, "%d", pin) ;
if ((pid = fork ()) < 0) // Fail
return printf("wiringPiISR: fork failed: %s\n", strerror (errno)) ;
if (pid == 0) // Child, exec
{
/**/ if (access ("/usr/local/bin/gpio", X_OK) == 0)
{
execl ("/usr/local/bin/gpio", "gpio", "edge", pinS, modeS, (char *)NULL) ;
return printf ("wiringPiISR: execl failed: %s\n", strerror (errno)) ;
}
else if (access ("/usr/bin/gpio", X_OK) == 0)
{
execl ("/usr/bin/gpio", "gpio", "edge", pinS, modeS, (char *)NULL) ;
return printf ("wiringPiISR: execl failed: %s\n", strerror (errno)) ;
}
else
return printf ("wiringPiISR: Can't find gpio program\n") ;
}
else // Parent, wait
wait (NULL) ;
return 1;
}
void rfNoInterrupts(){
pthread_mutex_lock (&pinMutex) ;
}
......@@ -179,13 +224,3 @@ void rfNoInterrupts(){
void rfInterrupts(){
pthread_mutex_unlock (&pinMutex) ;
}
\ No newline at end of file
void spiNoInterrupts(){
pthread_mutex_lock (&spiMutex) ;
}
void spiInterrupts(){
pthread_mutex_unlock (&spiMutex) ;
}
......@@ -52,10 +52,18 @@ extern int piHiPri (const int pri);
*********************************************************************************
*/
extern int attachInterrupt (int pin, int mode, void (*function)(void));
/*
* detachInterrupt:
* Pi Specific detachInterrupt.
* Will cancel the interrupt thread, close the filehandle and
* setting wiringPi back to 'none' mode.
*********************************************************************************
*/
extern int detachInterrupt (int pin);
extern void rfNoInterrupts();
extern void rfInterrupts();
extern void spiNoInterrupts();
extern void spiInterrupts();
#ifdef __cplusplus
}
#endif
\ No newline at end of file
#include "spi.h"
#include <pthread.h>
static pthread_mutex_t spiMutex;
SPI::SPI() {
......@@ -8,42 +9,40 @@ SPI::SPI() {
void SPI::begin( int busNo ) {
spiNoInterrupts();
if (!bcm2835_init()){
spiInterrupts();
return;
}
bcm2835_spi_begin();
spiInterrupts();
}
void SPI::end() {
void SPI::beginTransaction(SPISettings settings){
pthread_mutex_lock (&spiMutex);
setBitOrder(settings.border);
setDataMode(settings.dmode);
setClockDivider(settings.clck);
}
void SPI::endTransaction() {
pthread_mutex_unlock (&spiMutex);
}
void SPI::setBitOrder(uint8_t bit_order) {
spiNoInterrupts();
bcm2835_spi_setBitOrder(bit_order);
spiInterrupts();
}
void SPI::setDataMode(uint8_t data_mode) {
spiNoInterrupts();
bcm2835_spi_setDataMode(data_mode);
spiInterrupts();
}
void SPI::setClockDivider(uint16_t spi_speed) {
spiNoInterrupts();
bcm2835_spi_setClockDivider(spi_speed);
spiInterrupts();
}
void SPI::chipSelect(int csn_pin){
spiNoInterrupts();
bcm2835_spi_chipSelect(csn_pin);
spiInterrupts();
delayMicroseconds(5);
}
SPI::~SPI() {
......
......@@ -14,6 +14,31 @@
#include "bcm2835.h"
#include "interrupt.h"
#define SPI_HAS_TRANSACTION
#define MSBFIRST BCM2835_SPI_BIT_ORDER_MSBFIRST
#define SPI_MODE0 BCM2835_SPI_MODE0
#define RF24_SPI_SPEED BCM2835_SPI_SPEED_8MHZ
class SPISettings {
public:
SPISettings(uint32_t clock, uint8_t bitOrder, uint8_t dataMode) {
init(clock,bitOrder,dataMode);
}
SPISettings() { init(RF24_SPI_SPEED, MSBFIRST, SPI_MODE0); }
uint32_t clck;
uint8_t border;
uint8_t dmode;
private:
void init(uint32_t clock, uint8_t bitOrder, uint8_t dataMode) {
clck = clock;
border = bitOrder;
dmode = dataMode;
}
friend class SPIClass;
};
class SPI {
public:
......@@ -32,20 +57,21 @@ public:
static void setDataMode(uint8_t data_mode);
static void setClockDivider(uint16_t spi_speed);
static void chipSelect(int csn_pin);
static void beginTransaction(SPISettings settings);
static void endTransaction();
};
uint8_t SPI::transfer(uint8_t _data) {
spiNoInterrupts();
uint8_t data = bcm2835_spi_transfer(_data);
spiInterrupts();
return data;
}
void SPI::transfernb(char* tbuf, char* rbuf, uint32_t len){
spiNoInterrupts();
bcm2835_spi_transfernb( tbuf, rbuf, len);
spiInterrupts();
}
void SPI::transfern(char* buf, uint32_t len)
......
Markdown is supported
0%
or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment