@ -74,18 +74,14 @@ using std::this_thread::sleep_for;
//////////////////////////////////////
const int32_t CanDriver : : CAN_MAX_DATA_LENGTH = 8 ;
const int32_t CanDriver : : CAN_SOCK_RAW = CAN_RAW ;
const int32_t CanDriver : : CAN_SOCK_SEVEN = 7 ;
const int32_t CanDriver : : CAN_SOCK_RAW = CAN_RAW ;
const int32_t CanDriver : : CAN_SOCK_SEVEN = 7 ;
CanDriver : : CanDriver ( string canInterface , int32_t canProtocol ,
const CanId defaultSenderId )
: CanDriver ( canInterface , canProtocol , 0 /* match all */ , defaultSenderId ) {
}
CanDriver : : CanDriver ( string canInterface , int32_t canProtocol , const CanId defaultSenderId )
: CanDriver ( canInterface , canProtocol , 0 /* match all */ , defaultSenderId ) { }
CanDriver : : CanDriver ( const string canInterface , const int32_t canProtocol ,
const int32_t filterMask , const CanId defaultSenderId )
: _defaultSenderId ( defaultSenderId ) , _canProtocol ( canProtocol ) ,
_canInterface ( canInterface ) , _canFilterMask ( filterMask ) , _socketFd ( - 1 ) {
CanDriver : : CanDriver ( const string canInterface , const int32_t canProtocol , const int32_t filterMask , const CanId defaultSenderId )
: _defaultSenderId ( defaultSenderId ) , _canProtocol ( canProtocol ) , _canInterface ( canInterface ) , _canFilterMask ( filterMask ) , _socketFd ( - 1 ) {
initialiseSocketCan ( ) ;
}
@ -105,9 +101,9 @@ bool CanDriver::waitForMessages(milliseconds timeout) {
unique_lock < mutex > locky ( _lock ) ;
fd_set readFileDescriptors ;
fd_set readFileDescriptors ;
timeval waitTime ;
waitTime . tv_sec = timeout . count ( ) / 1000 ;
waitTime . tv_sec = timeout . count ( ) / 1000 ;
waitTime . tv_usec = timeout . count ( ) * 1000 ;
FD_ZERO ( & readFileDescriptors ) ;
@ -132,19 +128,13 @@ CanMessage CanDriver::readMessage() { return readMessageLock(); }
*/
CanMessage CanDriver : : readMessageLock ( bool const lock ) {
std : : unique_ptr < std : : unique_lock < std : : mutex > > _lockLck { nullptr } ;
if ( lock )
_lockLck = std : : unique_ptr < std : : unique_lock < std : : mutex > > {
new std : : unique_lock < std : : mutex > { _lock } } ;
if ( 0 > _socketFd )
throw InvalidSocketException ( " Invalid socket! " , _socketFd ) ;
int32_t readBytes { 0 } ;
if ( lock ) _lockLck = std : : unique_ptr < std : : unique_lock < std : : mutex > > { new std : : unique_lock < std : : mutex > { _lock } } ;
if ( 0 > _socketFd ) throw InvalidSocketException ( " Invalid socket! " , _socketFd ) ;
int32_t readBytes { 0 } ;
can_frame canFrame ;
memset ( & canFrame , 0 , sizeof ( can_frame ) ) ;
readBytes = read ( _socketFd , & canFrame , sizeof ( can_frame ) ) ;
if ( 0 > readBytes )
throw CanException ( formatString ( " FAILED to read from CAN! Error: %d => %s " ,
errno , strerror ( errno ) ) ,
_socketFd ) ;
if ( 0 > readBytes ) throw CanException ( formatString ( " FAILED to read from CAN! Error: %d => %s " , errno , strerror ( errno ) ) , _socketFd ) ;
return CanMessage { canFrame } ;
}
@ -166,11 +156,7 @@ int32_t CanDriver::sendMessage(const CanMessage message, bool forceExtended) {
int32_t bytesWritten = 0 ;
if ( message . getFrameData ( ) . size ( ) > CAN_MAX_DATA_LENGTH ) {
throw CanException (
formatString (
" INVALID data length! Message must be smaller than %d bytes! " ,
CAN_MAX_DATA_LENGTH ) ,
_socketFd ) ;
throw CanException ( formatString ( " INVALID data length! Message must be smaller than %d bytes! " , CAN_MAX_DATA_LENGTH ) , _socketFd ) ;
}
auto canFrame = message . getRawFrame ( ) ;
@ -182,10 +168,7 @@ int32_t CanDriver::sendMessage(const CanMessage message, bool forceExtended) {
bytesWritten = write ( _socketFd , ( const void * ) & canFrame , sizeof ( canFrame ) ) ;
if ( bytesWritten = = - 1 ) {
throw CanException (
formatString ( " FAILED to write data to socket! Error: %d => %s " , errno ,
strerror ( errno ) ) ,
_socketFd ) ;
throw CanException ( formatString ( " FAILED to write data to socket! Error: %d => %s " , errno , strerror ( errno ) ) , _socketFd ) ;
}
return bytesWritten ;
@ -200,8 +183,7 @@ int32_t CanDriver::sendMessage(const CanMessage message, bool forceExtended) {
*
* @ return int32_t The total amount of bytes sent .
*/
int32_t CanDriver : : sendMessageQueue ( queue < CanMessage > messages ,
milliseconds delay , bool forceExtended ) {
int32_t CanDriver : : sendMessageQueue ( queue < CanMessage > messages , milliseconds delay , bool forceExtended ) {
if ( _socketFd < 0 ) {
throw InvalidSocketException ( " Invalid socket! " , _socketFd ) ;
}
@ -224,12 +206,10 @@ int32_t CanDriver::sendMessageQueue(queue<CanMessage> messages,
* buffer .
*/
queue < CanMessage > CanDriver : : readQueuedMessages ( ) {
if ( _socketFd < 0 )
throw InvalidSocketException ( " Invalid socket! " , _socketFd ) ;
if ( _socketFd < 0 ) throw InvalidSocketException ( " Invalid socket! " , _socketFd ) ;
unique_lock < mutex > locky ( _lock ) ;
queue < CanMessage > messages ;
for ( int32_t i = _queueSize ; 0 < i ; - - i )
messages . emplace ( readMessageLock ( false ) ) ;
queue < CanMessage > messages ;
for ( int32_t i = _queueSize ; 0 < i ; - - i ) messages . emplace ( readMessageLock ( false ) ) ;
return messages ;
}
@ -244,16 +224,13 @@ void CanDriver::setCanFilterMask(const int32_t mask) {
}
unique_lock < mutex > locky ( _lock ) ;
can_filter canFilter ;
can_filter canFilter ;
canFilter . can_id = _defaultSenderId ;
canFilter . can_id = _defaultSenderId ;
canFilter . can_mask = mask ;
if ( setsockopt ( _socketFd , SOL_CAN_RAW , CAN_RAW_FILTER , & canFilter ,
sizeof ( canFilter ) ) = = - 1 ) {
throw CanInitException ( formatString (
" FAILED to set CAN filter mask %x on socket %d! Error: %d => %s " , mask ,
_socketFd , errno , strerror ( errno ) ) ) ;
if ( setsockopt ( _socketFd , SOL_CAN_RAW , CAN_RAW_FILTER , & canFilter , sizeof ( canFilter ) ) = = - 1 ) {
throw CanInitException ( formatString ( " FAILED to set CAN filter mask %x on socket %d! Error: %d => %s " , mask , _socketFd , errno , strerror ( errno ) ) ) ;
}
_canFilterMask = mask ;
@ -270,9 +247,9 @@ void CanDriver::initialiseSocketCan() {
// unique_lock<mutex> locky(_lock);
struct sockaddr_can address ;
struct ifreq ifaceRequest ;
int64_t fdOptions = 0 ;
int32_t tmpReturn ;
struct ifreq ifaceRequest ;
int64_t fdOptions = 0 ;
int32_t tmpReturn ;
memset ( & address , 0 , sizeof ( sizeof ( struct sockaddr_can ) ) ) ;
memset ( & ifaceRequest , 0 , sizeof ( sizeof ( struct ifreq ) ) ) ;
@ -280,33 +257,26 @@ void CanDriver::initialiseSocketCan() {
_socketFd = socket ( PF_CAN , SOCK_RAW , _canProtocol ) ;
if ( _socketFd = = - 1 ) {
throw CanInitException (
formatString ( " FAILED to initialise socketcan! Error: %d => %s " , errno ,
strerror ( errno ) ) ) ;
throw CanInitException ( formatString ( " FAILED to initialise socketcan! Error: %d => %s " , errno , strerror ( errno ) ) ) ;
}
strcpy ( ifaceRequest . ifr_name , _canInterface . c_str ( ) ) ;
if ( ( tmpReturn = ioctl ( _socketFd , SIOCGIFINDEX , & ifaceRequest ) ) = = - 1 ) {
throw CanInitException ( formatString (
" FAILED to perform IO control operation on socket %s! Error: %d => %s " ,
_canInterface . c_str ( ) , errno , strerror ( errno ) ) ) ;
throw CanInitException ( formatString ( " FAILED to perform IO control operation on socket %s! Error: %d => %s " , _canInterface . c_str ( ) , errno , strerror ( errno ) ) ) ;
}
fdOptions = fcntl ( _socketFd , F_GETFL ) ;
fdOptions | = O_NONBLOCK ;
tmpReturn = fcntl ( _socketFd , F_SETFL , fdOptions ) ;
address . can_family = AF_CAN ;
address . can_family = AF_CAN ;
address . can_ifindex = ifaceRequest . ifr_ifindex ;
setCanFilterMask ( _canFilterMask ) ;
if ( ( tmpReturn = bind ( _socketFd , ( struct sockaddr * ) & address ,
sizeof ( address ) ) ) = = - 1 ) {
throw CanInitException (
formatString ( " FAILED to bind to socket CAN! Error: %d => %s " , errno ,
strerror ( errno ) ) ) ;
if ( ( tmpReturn = bind ( _socketFd , ( struct sockaddr * ) & address , sizeof ( address ) ) ) = = - 1 ) {
throw CanInitException ( formatString ( " FAILED to bind to socket CAN! Error: %d => %s " , errno , strerror ( errno ) ) ) ;
}
}
@ -321,11 +291,10 @@ void CanDriver::uninitialiseSocketCan() {
}
if ( close ( _socketFd ) = = - 1 ) {
throw CanCloseException ( formatString (
" FAILED to close CAN socket! Error: %d => %s " , errno , strerror ( errno ) ) ) ;
throw CanCloseException ( formatString ( " FAILED to close CAN socket! Error: %d => %s " , errno , strerror ( errno ) ) ) ;
}
_socketFd = - 1 ;
}
} // namespace sockcanpp
} // namespace sockcanpp