ecmc_plugin_socketcan/src/ecmcSocketCANWriteBuffer.cpp

/*************************************************************************\
* Copyright (c) 2019 European Spallation Source ERIC
* ecmc is distributed subject to a Software License Agreement found
* in file LICENSE that is included with this distribution. 
*
*  ecmcSocketCANWriteBuffer.cpp
*
*  Created on: Mar 22, 2020
*      Author: anderssandstrom
*
\*************************************************************************/

// Needed to get headers in ecmc right...
#define ECMC_IS_PLUGIN

#include <sstream>
#include "ecmcSocketCANWriteBuffer.h"
#include "epicsThread.h"

#define ECMC_PLUGIN_ASYN_PREFIX      "plugin.can"

// Start worker for socket read()
void f_worker_write(void *obj) {
  if(!obj) {
    printf("%s/%s:%d: Error: Worker write thread ecmcSocketCANWriteBuffer object NULL..\n",
            __FILE__, __FUNCTION__, __LINE__);
    return;
  }
  ecmcSocketCANWriteBuffer * canObj = (ecmcSocketCANWriteBuffer*)obj;
  canObj->doWriteWorker();
}

/** ecmc ecmcSocketCANWriteBuffer class
 * Implements writing of can messages to a socket. 
 * Two buffers are used. While a thread writes to socket from one of teh buffers,
 * data can still be added to the other buffer. Then teh buffers are switched.
*/
ecmcSocketCANWriteBuffer::ecmcSocketCANWriteBuffer(int socketId, int cfgDbgMode) {
  memset(&buffer1_.frames,0,sizeof(struct can_frame)*ECMC_CAN_MAX_WRITE_CMDS);
  memset(&buffer2_.frames,0,sizeof(struct can_frame)*ECMC_CAN_MAX_WRITE_CMDS);
  bufferIdAddFrames_ = 1;  // start to add frames to buffer 1
  writeBusy_         = 0;
  socketId_          = socketId;
  cfgDbgMode_        = cfgDbgMode;
  destructs_         = 0;
  bufferSwitchMutex_ = epicsMutexCreate();
  lastWriteSumError_ = 0;
  writePauseTime_.tv_sec  = 0;
  writePauseTime_.tv_nsec = 2e6;  // 2ms
  buffer1_.frameCounter = 0;
  buffer2_.frameCounter = 0;

  bufferAdd_ = &buffer1_;
  bufferWrite_ = &buffer2_; 

  // Create worker thread for writing socket
  std::string threadname = "ecmc." ECMC_PLUGIN_ASYN_PREFIX".write";
  if(epicsThreadCreate(threadname.c_str(), 0, 32768, f_worker_write, this) == NULL) {
    throw std::runtime_error("Error: Failed create worker thread for write().");
  }
}

ecmcSocketCANWriteBuffer::~ecmcSocketCANWriteBuffer() {
  // kill worker
  destructs_ = 1;  // maybe need todo in other way..
}

// Write socket worker thread (switch between two buffers)
void ecmcSocketCANWriteBuffer::doWriteWorker() {
  while(true) {    
    if(destructs_) {
      return;
    }    

    nanosleep(&writePauseTime_,NULL);    

    if(writeBusy_) {      
      continue;
    }

    if(destructs_) {
      return;
    }

    writeBusy_ = 1;
    // Check if anything to write..
    if(bufferAdd_->frameCounter == 0) {
      writeBusy_ = 0;
      continue;
    }

    // Switch buffers and write!
    switchBuffer();
    
    writeBuffer();
    writeBusy_ = 0;
  }
}

int ecmcSocketCANWriteBuffer::addWriteCAN(can_frame *frame) {  
  // Cannot switch if busy..
  int errorCode = 0;
  epicsMutexLock(bufferSwitchMutex_);
  errorCode = addToBuffer(frame);
  epicsMutexUnlock(bufferSwitchMutex_);
  return errorCode;
}

// Test can write function (simple if for plc func)
int ecmcSocketCANWriteBuffer::addWriteCAN( uint32_t canId,
                                           uint8_t len,
                                           uint8_t data0,
                                           uint8_t data1,
                                           uint8_t data2,
                                           uint8_t data3,
                                           uint8_t data4,
                                           uint8_t data5,
                                           uint8_t data6,
                                           uint8_t data7) {
  can_frame frame;
  frame.can_id  = canId;
  frame.can_dlc = len;     // data length
  frame.data[0] = data0;  // request read cmd
  frame.data[1] = data1;
  frame.data[2] = data2;
  frame.data[3] = data3;
  frame.data[4] = data4;
  frame.data[5] = data5;
  frame.data[6] = data6;
  frame.data[7] = data7;
  return addWriteCAN(&frame);
}

int ecmcSocketCANWriteBuffer::getlastWritesErrorAndReset() {
  int tempError = lastWriteSumError_;
  lastWriteSumError_ = 0;
  return tempError;
}

int ecmcSocketCANWriteBuffer::addToBuffer(can_frame *frame) {

  if(bufferAdd_->frameCounter >= ECMC_CAN_MAX_WRITE_CMDS) {
    return ECMC_CAN_ERROR_WRITE_FULL;
  }

  bufferAdd_->frames[bufferAdd_->frameCounter] = *frame;
  bufferAdd_->frameCounter++; 
  return 0;
}

int ecmcSocketCANWriteBuffer::writeBuffer() {

  int errorCode = 0;
  if(bufferWrite_->frameCounter==0) {
    return 0;
  }

  for(int i=0; i<bufferWrite_->frameCounter;i++) {
    errorCode = writeCAN(&bufferWrite_->frames[i]);
    if(errorCode) {
      lastWriteSumError_ = errorCode;
    }
  }
  bufferWrite_->frameCounter = 0;
  return lastWriteSumError_;
}

int ecmcSocketCANWriteBuffer::switchBuffer() {

  // ensure safe buffer switch
  epicsMutexLock(bufferSwitchMutex_);
  canWriteBuffer *temp = bufferWrite_;
  bufferWrite_ = bufferAdd_;
  bufferAdd_   = temp;
  epicsMutexUnlock(bufferSwitchMutex_);
  return 0;
}

// Write to socket
int ecmcSocketCANWriteBuffer::writeCAN(can_frame *frame){

  if(!frame) {
    return ECMC_CAN_ERROR_WRITE_NO_DATA;
  }

  // Maybe need to add the size to write here.. if struct is not full, hmm?!
  int nbytes = write(socketId_, frame, sizeof(struct can_frame));

  if(nbytes == -1) {    
    printf("ecmcSocketCAN: write() fail with error: %s (0x%x).\n", strerror(errno),errno);
    return errno;    
  }
  
  if (nbytes!= sizeof(struct can_frame)) {
    printf("ecmcSocketCAN: write() fail with error: Incomplete write(), not a full can frame (0x%x).\n",ECMC_CAN_ERROR_WRITE_INCOMPLETE);    
    return ECMC_CAN_ERROR_WRITE_INCOMPLETE;
  }

  if(cfgDbgMode_) {	  
    // Simulate candump printout
    printf("w 0x%03X", frame->can_id);
    printf(" [%d]", frame->can_dlc);
    for(int i=0; i<frame->can_dlc; i++ ) {
      printf(" 0x%02X", frame->data[i]);
    }
    printf("\n");
  }
  return 0;
}

// Avoid issues with std:to_string()
std::string ecmcSocketCANWriteBuffer::to_string(int value) {
  std::ostringstream os;
  os << value;
  return os.str();
}