/**
 * Project:      Open Vehicle Monitor System
 * Module:       CANopen
 * 
 * (c) 2017  Michael Balzer <dexter@dexters-web.de>
 * 
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 * 
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 * 
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 */

#include "ovms_log.h"
static const char *TAG = "canopen";

#include "ovms_metrics.h"
#include "metrics_standard.h"
#include "ovms_events.h"
#include "canopen.h"


// SDO commands:

#define SDO_CommandMask             0b11100000
#define SDO_ExpeditedSizeMask       0b00001100
#define SDO_Expedited               0b00000010
#define SDO_SizeIndicated           0b00000001

#define SDO_Abort                   0b10000000

#define SDO_InitUploadRequest       0b01000000
#define SDO_InitUploadResponse      0b01000000
#define SDO_UploadSegmentRequest    0b01100000
#define SDO_UploadSegmentResponse   0b00000000

#define SDO_InitDownloadRequest     0b00100000
#define SDO_InitDownloadResponse    0b01100000
#define SDO_DownloadSegmentRequest  0b00000000
#define SDO_DownloadSegmentResponse 0b00100000

#define SDO_SegmentToggle           0b00010000
#define SDO_SegmentUnusedMask       0b00001110
#define SDO_SegmentEnd              0b00000001

// SDO abort reasons:

#define SDO_Abort_SegMismatch       0x05030000
#define SDO_Abort_Timeout           0x05040000
#define SDO_Abort_OutOfMemory       0x05040005


static void CANopenWorkerJobTask(void *pvParameters);


/**
 * A CANopenWorker processes CANopenJobs on a specific bus.
 * 
 * CANopenClients create and submit Jobs to be processed to a CANopenWorker.
 * After finish/abort, the Worker sends the Job to the clients done queue.
 * 
 * A CANopenWorker also monitors the bus for emergency and heartbeat
 * messages, and translates these into events and metrics updates.
 */

CANopenWorker::CANopenWorker(canbus* bus)
  {
  m_bus = bus;
  
  m_clientcnt = 0;
  
  m_nmt_rxcnt = 0;
  m_emcy_rxcnt = 0;
  
  m_jobcnt = 0;
  m_jobcnt_timeout = 0;
  m_jobcnt_error = 0;
  
  m_jobqueue = xQueueCreate(20, sizeof(CANopenJob));
  snprintf(m_taskname, sizeof(m_taskname), "OVMS COwrk %s", bus->GetName());
  xTaskCreatePinnedToCore(CANopenWorkerJobTask, m_taskname,
    CONFIG_OVMS_COMP_CANOPEN_WRK_STACK, (void*)this, 15, &m_jobtask, CORE(0));
  
  memset(&m_job, 0, sizeof(m_job));
  m_job.type = COJT_None;
  
  memset(&m_request, 0, sizeof(m_request));
  memset(&m_response, 0, sizeof(m_response));
  }

CANopenWorker::~CANopenWorker()
  {
  vQueueDelete(m_jobqueue);
  vTaskDelete(m_jobtask);
  }


void CANopenWorker::Open(CANopenAsyncClient* client)
  {
  m_clients.push_front(client);
  ++m_clientcnt;
  }

void CANopenWorker::Close(CANopenAsyncClient* client)
  {
  m_clients.remove(client);
  m_clientcnt ? --m_clientcnt : 0;
  }

bool CANopenWorker::IsClient(CANopenAsyncClient* client)
  {
  for (auto c : m_clients)
    {
    if (c == client)
      return true;
    }
  return false;
  }


void CANopenWorker::StatusReport(int verbosity, OvmsWriter* writer)
  {
  writer->printf(
    "  %s:\n"
    "    Active clients: %d\n"
    "    Jobs waiting  : %d\n"
    "    Jobs processed: %d\n"
    "    - timeouts    : %d\n"
    "    - other errors: %d\n"
    "    NMT received  : %d\n"
    "    EMCY received : %d\n"
    , m_bus->GetName()
    , m_clientcnt
    , (int)uxQueueMessagesWaiting(m_jobqueue)
    , m_jobcnt
    , m_jobcnt_timeout
    , m_jobcnt_error
    , m_nmt_rxcnt
    , m_emcy_rxcnt);
  }


/**
 * GetNodeMetrics: find / create metrics bundle for a nodeid
 */
CANopenNodeMetrics *CANopenWorker::GetNodeMetrics(uint8_t nodeid)
  {
  CANopenNodeMetrics *nm = m_nodemetrics[nodeid];
  
  if (!nm)
    {
    // add node metrics:
    nm = new CANopenNodeMetrics();
    char name[50];
    
    snprintf(name, sizeof(name), "co.%s.nd%d.state", m_bus->GetName(), nodeid);
    if (!(nm->m_state = (OvmsMetricString*) MyMetrics.Find(name)))
      nm->m_state = new OvmsMetricString(strdup(name), SM_STALE_MIN);
    
    snprintf(name, sizeof(name), "co.%s.nd%d.emcy.code", m_bus->GetName(), nodeid);
    if (!(nm->m_emcy_code = (OvmsMetricInt*) MyMetrics.Find(name)))
      nm->m_emcy_code = new OvmsMetricInt(strdup(name), SM_STALE_MIN);
    
    snprintf(name, sizeof(name), "co.%s.nd%d.emcy.type", m_bus->GetName(), nodeid);
    if (!(nm->m_emcy_type = (OvmsMetricInt*) MyMetrics.Find(name)))
      nm->m_emcy_type = new OvmsMetricInt(strdup(name), SM_STALE_MIN);
    
    m_nodemetrics[nodeid] = nm;
    }
  
  return nm;
  }


/**
 * SubmitJob: post a new job to the JobTask queue
 */
CANopenResult_t CANopenWorker::SubmitJob(CANopenJob& job, TickType_t maxqueuewait /*=0*/)
  {
  if (xQueueSend(m_jobqueue, &job, maxqueuewait) != pdTRUE)
    job.result = COR_ERR_QueueFull;
  else
    job.result = COR_WAIT;
  return job.result;
  }


/**
 * JobTask: process CANopenJobs, send results back to clients
 */

static void CANopenWorkerJobTask(void *pvParameters)
  {
  CANopenWorker *me = (CANopenWorker*)pvParameters;
  me->JobTask();
  }

void CANopenWorker::JobTask()
  {
  while(1)
    {
    // get next job:
    if (xQueueReceive(m_jobqueue, &m_job, (portTickType)portMAX_DELAY) == pdTRUE)
      {
        // check client:
        if (!IsClient(m_job.client))
          {
          ESP_LOGW(TAG, "Job dropped: Client vanished");
          continue;
          }
        
        // process job:
        switch (m_job.type)
          {
          case COJT_None:
            m_job.result = COR_OK;
            break;
          case COJT_SendNMT:
            ESP_LOGV(TAG, "SendNMT: %s node=%d, command=%d", m_bus->GetName(), m_job.nmt.nodeid, m_job.nmt.command);
            m_job.result = ProcessSendNMTJob();
            ESP_LOGV(TAG, "SendNMT result: %s", CANopen::GetResultString(m_job).c_str());
            break;
          case COJT_ReceiveHB:
            ESP_LOGV(TAG, "ReceiveHB: %s node=%d", m_bus->GetName(), m_job.hb.nodeid);
            m_job.result = ProcessReceiveHBJob();
            ESP_LOGV(TAG, "ReceiveHB result: %s", CANopen::GetResultString(m_job).c_str());
            break;
          case COJT_ReadSDO:
            ESP_LOGV(TAG, "ReadSDO: %s node=%d adr=%04x.%02x", m_bus->GetName(), m_job.sdo.nodeid, m_job.sdo.index, m_job.sdo.subindex);
            m_job.result = ProcessReadSDOJob();
            ESP_LOGV(TAG, "ReadSDO result: %s", CANopen::GetResultString(m_job).c_str());
            break;
          case COJT_WriteSDO:
            ESP_LOGV(TAG, "WriteSDO: %s node=%d adr=%04x.%02x", m_bus->GetName(), m_job.sdo.nodeid, m_job.sdo.index, m_job.sdo.subindex);
            m_job.result = ProcessWriteSDOJob();
            ESP_LOGV(TAG, "WriteSDO result: %s", CANopen::GetResultString(m_job).c_str());
            break;
          default:
            ESP_LOGW(TAG, "Unknown job type: %d", (int)m_job.type);
            m_job.result = COR_ERR_UnknownJobType;
          }
        
        // return job to client if still valid:
        if (!IsClient(m_job.client))
          {
          ESP_LOGW(TAG, "Job result lost: Client vanished");
          }
        else
          {
          if (m_job.client->SubmitDoneCallback(m_job, 0) != COR_OK)
            ESP_LOGW(TAG, "Job result lost: Client queue is full");
          }
        
        // statistics:
        m_jobcnt++;
        if (m_job.result == COR_ERR_Timeout)
          m_jobcnt_timeout++;
        else if (m_job.result != COR_OK)
          m_jobcnt_error++;
        
        m_job.type = COJT_None;
      }
    }
  }


/**
 * IncomingFrame: process EMCY and Heartbeat messages, forward job frames to job task
 */
void CANopenWorker::IncomingFrame(CAN_frame_t* p_frame)
  {
  // Message matching our current job?
  if (m_job.type != COJT_None && p_frame->MsgID == m_job.rxid)
    {
    // copy payload into m_response:
    int i;
    for (i=0; i < p_frame->FIR.B.DLC; i++)
      m_response.byte[i] = p_frame->data.u8[i];
    for (; i < 8; i++)
      m_response.byte[i] = 0;
    
    // signal job task:
    xTaskNotifyGive(m_jobtask);
    }
  
  
  // EMCY (Emergency) message?
  if (p_frame->MsgID > 0x080 && p_frame->MsgID < 0x100 && p_frame->FIR.B.DLC == 8)
    {
    m_emcy_rxcnt++;
    
    CANopenEMCYEvent ev;
    ev.origin = p_frame->origin;
    ev.nodeid = p_frame->MsgID - 0x080;
    ev.code = p_frame->data.u8[0] | (p_frame->data.u8[1] << 8);
    ev.type = p_frame->data.u8[2];
    memcpy(ev.data, p_frame->data.u8+3, 5);
    
    CANopenNodeMetrics *nm = GetNodeMetrics(ev.nodeid);
    
    // Note: logging is very expensive if file logging to SD card is enabled (→ issue #107).
    //  Workaround: level down to verbose (should be info):
    ESP_LOGV(TAG, "%s node %d emergency: code=0x%04x type=0x%02x data: %02x %02x %02x %02x %02x",
      m_bus->GetName(), ev.nodeid, ev.code, ev.type, ev.data[0], ev.data[1], ev.data[2], ev.data[3], ev.data[4]);
    MyEvents.SignalEvent("canopen.node.emcy", &ev, sizeof(ev));
    nm->m_emcy_code->SetValue(ev.code);
    nm->m_emcy_type->SetValue(ev.type);
    }
  
  // NMT (Heartbeat/State) message?
  else if (p_frame->MsgID > 0x700 && p_frame->MsgID < 0x780 && p_frame->FIR.B.DLC == 1)
    {
    m_nmt_rxcnt++;
    
    CANopenNMTEvent ev;
    ev.origin = p_frame->origin;
    ev.nodeid = p_frame->MsgID - 0x700;
    ev.state = (CANopenNMTState_t) p_frame->data.u8[0];
    
    CANopenNodeMetrics *nm = GetNodeMetrics(ev.nodeid);
    
    std::string newstate = CANopen::GetStateName(ev.state);
    if (nm->m_state->AsString() != newstate)
      {
      // Note: logging is very expensive if file logging to SD card is enabled (→ issue #107).
      //  Workaround: level down to verbose (should be info):
      ESP_LOGV(TAG, "%s node %d new state: %s", m_bus->GetName(), ev.nodeid, newstate.c_str());
      MyEvents.SignalEvent("canopen.node.state", &ev, sizeof(ev));
      nm->m_state->SetValue(newstate);
      }
    else
      {
      nm->m_state->SetStale(false);
      }
    }
  
  } // IncomingFrame()


/**
 * ProcessSendNMTJob: send NMT request and optionally wait for NMT state change
 *  a.k.a. heartbeat message.
 * 
 * Note: NMT responses are not a part of the CANopen NMT protocol, and
 *  sending "heartbeat" NMT state updates is optional for CANopen nodes.
 *  If the node sends no state info, waiting for it will result in timeout
 *  even though the state has in fact changed -- there's no way to know
 *  if the node doesn't tell.
 */
CANopenResult_t CANopenWorker::ProcessSendNMTJob()
  {
  // check bus:
  if (m_bus->m_mode != CAN_MODE_ACTIVE)
    return COR_ERR_NoCANWrite;
  
  // check parameters:
  if (m_job.nmt.nodeid > 127)
    return COR_ERR_ParamRange;
  
  // init request:
  CAN_frame_t txframe;
  memset(&txframe, 0, sizeof(txframe));
  
  txframe.origin = m_bus;
  txframe.FIR.B.FF = CAN_frame_std;
  txframe.FIR.B.DLC = 2;
  txframe.MsgID = m_job.txid;
  txframe.data.u8[0] = (uint8_t) m_job.nmt.command;
  txframe.data.u8[1] = m_job.nmt.nodeid;
  
  TickType_t maxwait = pdMS_TO_TICKS(m_job.timeout_ms);
  
  do
    {
    // send request:
    m_job.trycnt++;
    txframe.Write();
    
    // immediate return?
    if (m_job.rxid == 0)
      return COR_OK;
    
    // wait for response signal from IncomingFrame():
    if (ulTaskNotifyTake(pdTRUE, maxwait))
      {
      // expected response for command?
      if ( (m_job.nmt.command == CONC_Start      && m_response.hb.state >= 5)
        || (m_job.nmt.command == CONC_Stop       && m_response.hb.state == 4)
        || (m_job.nmt.command == CONC_PreOp      && m_response.hb.state == 127)
        || (m_job.nmt.command == CONC_Reset      && m_response.hb.state != 4)
        || (m_job.nmt.command == CONC_CommReset  && m_response.hb.state != 4) )
        return COR_OK;
      }

    // timeout / wrong state → retry:
    if (m_job.trycnt < m_job.maxtries)
      vTaskDelay(pdMS_TO_TICKS(10));

    } while (m_job.trycnt < m_job.maxtries);

  return COR_ERR_Timeout;
  }


/**
 * ProcessReceiveHBJob: wait for next heartbeat message of a node,
 *  return state received.
 * 
 * Use this to read the current state or synchronize to the heartbeat.
 * Note: heartbeats are optional in CANopen.
 */
CANopenResult_t CANopenWorker::ProcessReceiveHBJob()
  {
  // check parameters:
  if (m_job.hb.nodeid < 1 || m_job.hb.nodeid > 127)
    return COR_ERR_ParamRange;
  
  TickType_t maxwait = pdMS_TO_TICKS(m_job.timeout_ms);
  
  do
    {
    m_job.trycnt++;
    
    // wait for receive signal from IncomingFrame():
    if (ulTaskNotifyTake(pdTRUE, maxwait))
      {
      // return state received:
      m_job.hb.state = (CANopenNMTState_t) m_response.hb.state;
      if (m_job.hb.statebuf)
        *m_job.hb.statebuf = m_job.hb.state;
      return COR_OK;
      }

    // timeout → retry:
    if (m_job.trycnt < m_job.maxtries)
      vTaskDelay(pdMS_TO_TICKS(10));

    } while (m_job.trycnt < m_job.maxtries);

  return COR_ERR_Timeout;
  }


/**
 * SendSDORequest: asynchronous tx of prepared CANopen SDO request
 */
void CANopenWorker::SendSDORequest()
  {
  // init tx frame:
  CAN_frame_t txframe;
  memset(&txframe, 0, sizeof(txframe));
  
  // load SDO request:
  txframe.origin = m_bus;
  txframe.FIR.B.FF = CAN_frame_std;
  txframe.FIR.B.DLC = 8;
  txframe.MsgID = m_job.txid;
  memcpy(txframe.data.u8, m_request.byte, 8);
  
  // send:
  txframe.Write();
  }


/**
 * AbortSDORequest: send SDO abort command
 */
void CANopenWorker::AbortSDORequest(uint32_t reason)
  {
  // backup request:
  uint8_t control = m_request.ctl.control;
  uint32_t data = m_request.ctl.data;
  
  // send abort:
  m_request.ctl.control = SDO_Abort;
  m_request.ctl.data = reason;
  SendSDORequest();
  
  // restore request:
  m_request.ctl.control = control;
  m_request.ctl.data = data;
  }


/**
 * ExecuteSDORequest: send SDO request and wait for response
 */
CANopenResult_t CANopenWorker::ExecuteSDORequest()
  {
  TickType_t maxwait = pdMS_TO_TICKS(m_job.timeout_ms);
  m_job.trycnt = 0;
  
  do
    {
    // send request:
    m_job.trycnt++;
    SendSDORequest();

    // wait for reply:
    if (ulTaskNotifyTake(pdTRUE, maxwait))
      return COR_OK;

    // timeout:
    AbortSDORequest(SDO_Abort_Timeout);
    if (m_job.trycnt < m_job.maxtries)
      vTaskDelay(pdMS_TO_TICKS(10));

    } while (m_job.trycnt < m_job.maxtries);
  
  return COR_ERR_Timeout;
  }


/**
 * ProcessReadSDOJob: read bytes from SDO server into buffer
 *   - reads data into m_job.sdo.buf (up to m_job.sdo.bufsize bytes)
 *   - returns data length read in m_job.sdo.xfersize
 *   - remaining buffer space will be zeroed
 *   - on result COR_ERR_BufferTooSmall, the buffer has been filled up to m_job.sdo.bufsize
 *   - on abort, the CANopen error code will be written into m_job.sdo.error
 * 
 * Note: result interpretation is up to caller (check device object dictionary for data types & sizes).
 *   As CANopen is little endian as ESP32, we don't need to check lengths on numerical results,
 *   i.e. anything from int8_t to uint32_t can simply be read into a uint32_t buffer.
 */
CANopenResult_t CANopenWorker::ProcessReadSDOJob()
  {
  // check for CAN write access:
  if (m_bus->m_mode != CAN_MODE_ACTIVE)
    return COR_ERR_NoCANWrite;

  // check parameters:
  if (m_job.sdo.nodeid < 1 || m_job.sdo.nodeid > 127)
    return COR_ERR_ParamRange;
  if (m_job.sdo.buf == NULL || m_job.sdo.bufsize == 0)
    return COR_ERR_ParamRange;
  
  uint8_t n, toggle, dlen;

  // init receive buffer:
  memset(m_job.sdo.buf, 0, m_job.sdo.bufsize);
  uint8_t *buf = m_job.sdo.buf;
  m_job.sdo.xfersize = 0;
  
  // request upload:
  memset(&m_request, 0, sizeof(m_request));
  m_request.exp.index = m_job.sdo.index;
  m_request.exp.subindex = m_job.sdo.subindex;
  m_request.exp.control = SDO_InitUploadRequest;
  if (ExecuteSDORequest() != COR_OK)
    {
    m_job.sdo.error = SDO_Abort_Timeout;
    return COR_ERR_Timeout;
    }

  // check response:
  if ((m_response.exp.control & SDO_CommandMask) != SDO_InitUploadResponse
    || m_response.exp.index != m_request.exp.index
    || m_response.exp.subindex != m_request.exp.subindex)
    {
    if ((m_response.exp.control & SDO_CommandMask) == SDO_Abort)
      m_job.sdo.error = m_response.ctl.data;
    else
      m_job.sdo.error = CANopen_BusCollision;
    ESP_LOGD(TAG, "ReadSDO #%d 0x%04x.%02x: InitUpload failed, CANopen error code 0x%08x",
      m_job.sdo.nodeid, m_job.sdo.index, m_job.sdo.subindex, m_job.sdo.error);
    return COR_ERR_SDO_Access;
    }

  // check for expedited xfer:
  if (m_response.exp.control & SDO_Expedited)
    {
    m_job.sdo.contsize = 4 - ((m_response.exp.control & SDO_ExpeditedSizeMask) >> 2);
    
    // copy response data to buffer:
    dlen = 8 - ((m_response.exp.control & SDO_ExpeditedSizeMask) >> 2);
    for (n = 4; n < dlen && m_job.sdo.xfersize < m_job.sdo.bufsize; n++, m_job.sdo.xfersize++)
      *buf++ = m_response.byte[n];
    if (n < dlen)
      {
      ESP_LOGD(TAG, "ReadSDO #%d 0x%04x.%02x: buffer too small, readlen=%d",
        m_job.sdo.nodeid, m_job.sdo.index, m_job.sdo.subindex, m_job.sdo.xfersize);
      m_job.sdo.error = SDO_Abort_OutOfMemory;
      return COR_ERR_BufferTooSmall;
      }
    
    return COR_OK;
    }

  // do segmented xfer:
  
  if (m_response.exp.control & SDO_SizeIndicated)
    m_job.sdo.contsize = m_response.ctl.data;
  else
    m_job.sdo.contsize = 0; // unknown size
  
  toggle = 0;
  memset(&m_request, 0, sizeof(m_request));
  do
    {
    // request next segment:
    m_request.seg.control = (SDO_UploadSegmentRequest|toggle);
    if (ExecuteSDORequest() != COR_OK)
      {
      m_job.sdo.error = SDO_Abort_Timeout;
      return COR_ERR_Timeout;
      }

    // check response:
    if ((m_response.seg.control & (SDO_CommandMask|SDO_SegmentToggle)) != (SDO_UploadSegmentResponse|toggle))
      {
      // mismatch → abort:
      ESP_LOGD(TAG, "ReadSDO #%d 0x%04x.%02x: segment mismatch, readlen=%d",
        m_job.sdo.nodeid, m_job.sdo.index, m_job.sdo.subindex, m_job.sdo.xfersize);
      AbortSDORequest(SDO_Abort_SegMismatch);
      m_job.sdo.error = SDO_Abort_SegMismatch;
      return COR_ERR_SDO_SegMismatch;
      }

    // ok, copy response data to buffer:
    dlen = 8 - ((m_response.seg.control & SDO_SegmentUnusedMask) >> 1);
    for (n = 1; n < dlen && m_job.sdo.xfersize < m_job.sdo.bufsize; n++, m_job.sdo.xfersize++)
      *buf++ = m_response.byte[n];
    if (n < dlen)
      {
      ESP_LOGD(TAG, "ReadSDO #%d 0x%04x.%02x: buffer too small, readlen=%d",
        m_job.sdo.nodeid, m_job.sdo.index, m_job.sdo.subindex, m_job.sdo.xfersize);
      if (!(m_response.seg.control & SDO_SegmentEnd))
        AbortSDORequest(SDO_Abort_OutOfMemory);
      m_job.sdo.error = SDO_Abort_OutOfMemory;
      return COR_ERR_BufferTooSmall;
      }

    // last segment fetched? => success!
    if (m_response.seg.control & SDO_SegmentEnd)
      return COR_OK;

    // toggle toggle bit:
    toggle = toggle ? 0 : SDO_SegmentToggle;

    } while(1);
  // not reached
  }


/**
 * ProcessWriteSDOJob: write bytes from buffer into SDO server
 *   - sends m_job.sdo.bufsize bytes from m_job.sdo.buf
 *   - … or 4 bytes from m_job.sdo.buf if bufsize is 0 (use for integer SDOs of unknown type)
 *   - returns data length sent in m_job.sdo.xfersize
 *   - on abort, the CANopen error code will be written into m_job.sdo.error
 * 
 * Note: the caller needs to know data type & size of the SDO register (check device object dictionary).
 *   As CANopen servers normally are intelligent, anything from int8_t to uint32_t can simply be
 *   sent as a uint32_t with bufsize=0, the server will know how to convert it.
 */
CANopenResult_t CANopenWorker::ProcessWriteSDOJob()
  {
  // check for CAN write access:
  if (m_bus->m_mode != CAN_MODE_ACTIVE)
    return COR_ERR_NoCANWrite;

  // check parameters:
  if (m_job.sdo.nodeid < 1 || m_job.sdo.nodeid > 127)
    return COR_ERR_ParamRange;
  if (m_job.sdo.buf == NULL)
    return COR_ERR_ParamRange;
  
  uint8_t n, toggle;
  
  // init send buffer:
  uint8_t *buf = m_job.sdo.buf;
  m_job.sdo.xfersize = 0;
  
  // request download:
  memset(&m_request, 0, sizeof(m_request));
  m_request.exp.index = m_job.sdo.index;
  m_request.exp.subindex = m_job.sdo.subindex;
  if (m_job.sdo.bufsize == 0)
    {
    // …expedited without size indication:
    m_request.exp.control = SDO_InitDownloadRequest | SDO_Expedited;
    for (n=0; n < 4; n++)
      m_request.exp.data[n] = *buf++;
    }
  else if (m_job.sdo.bufsize <= 4)
    {
    // …expedited with size indication:
    m_request.exp.control = SDO_InitDownloadRequest | SDO_Expedited | SDO_SizeIndicated | ((4-m_job.sdo.bufsize) << 2);
    for (n=0; n < m_job.sdo.bufsize; n++)
      m_request.exp.data[n] = *buf++;
    }
  else
    {
    // …segmented:
    m_request.exp.control = SDO_InitDownloadRequest | SDO_SizeIndicated;
    m_request.ctl.data = m_job.sdo.bufsize;
    }
  if (ExecuteSDORequest() != COR_OK)
    {
    m_job.sdo.error = SDO_Abort_Timeout;
    return COR_ERR_Timeout;
    }

  // check response:
  if ((m_response.exp.control & SDO_CommandMask) != SDO_InitDownloadResponse
    || m_response.exp.index != m_request.exp.index
    || m_response.exp.subindex != m_request.exp.subindex)
    {
    if ((m_response.exp.control & SDO_CommandMask) == SDO_Abort)
      m_job.sdo.error = m_response.ctl.data;
    else
      m_job.sdo.error = CANopen_BusCollision;
    ESP_LOGD(TAG, "WriteSDO #%d 0x%04x.%02x: InitDownload failed, CANopen error code 0x%08x",
      m_job.sdo.nodeid, m_job.sdo.index, m_job.sdo.subindex, m_job.sdo.error);
    return COR_ERR_SDO_Access;
    }
  
  // done?
  if (m_request.exp.control & SDO_Expedited)
    {
    m_job.sdo.xfersize = m_job.sdo.bufsize;
    return COR_OK;
    }

  // do segmented transfer:
  toggle = 0;
  do
    {
    // send next segment:
    for (n=0; n < 7 && m_job.sdo.xfersize < m_job.sdo.bufsize; n++, m_job.sdo.xfersize++)
      m_request.seg.data[n] = *buf++;
    m_request.seg.control = SDO_DownloadSegmentRequest | toggle | ((7-n) << 1);
    for (; n < 7; n++)
      m_request.seg.data[n] = 0;
    if (m_job.sdo.xfersize == m_job.sdo.bufsize)
      m_request.seg.control |= SDO_SegmentEnd;
    
    if (ExecuteSDORequest() != COR_OK)
      {
      m_job.sdo.error = SDO_Abort_Timeout;
      return COR_ERR_Timeout;
      }

    // check response:
    if ((m_response.seg.control & (SDO_CommandMask|SDO_SegmentToggle)) != (SDO_DownloadSegmentResponse|toggle))
      {
      // mismatch → abort:
      ESP_LOGD(TAG, "WriteSDO #%d 0x%04x.%02x: segment mismatch, readlen=%d",
        m_job.sdo.nodeid, m_job.sdo.index, m_job.sdo.subindex, m_job.sdo.xfersize);
      AbortSDORequest(SDO_Abort_SegMismatch);
      m_job.sdo.error = SDO_Abort_SegMismatch;
      return COR_ERR_SDO_SegMismatch;
      }

    // last segment sent? => success!
    if (m_request.seg.control & SDO_SegmentEnd)
      return COR_OK;

    // toggle toggle bit:
    toggle = toggle ? 0 : SDO_SegmentToggle;

    } while(1);
  // not reached
  }


