Add job types for local OD access

include/co_api.h

@@ -540,6 +540,40 @@ CO_EXPORT int co_error_clear (co_client_t * client, uint8_t mask);
  */
 CO_EXPORT int co_error_get (co_client_t * client, uint8_t * error);
 
+/**
+ * Read local dictionary object
+ *
+ * @param client        client handle
+ * @param index         index
+ * @param subindex      subindex
+ * @param value         result
+ *
+ * @return 0 on success, CO_STATUS error code otherwise
+ */
+CO_EXPORT int co_od_local_read (
+   co_client_t * client,
+   uint16_t index,
+   uint8_t subindex,
+   uint64_t * value);
+
+/**
+ * Write local directory object
+ *
+ * @param client        client handle
+ * @param index         index
+ * @param subindex      subindex
+ * @param value         value
+ * @param pdo_event     trigger PDOs that map this object
+ *
+ * @return 0 on success, CO_STATUS error code otherwise
+ */
+CO_EXPORT int co_od_local_write (
+   co_client_t * client,
+   uint16_t index,
+   uint8_t subindex,
+   uint64_t value,
+   bool pdo_event);
+
 #ifdef __cplusplus
 }
 #endif

src/co_main.c

@@ -33,6 +33,7 @@
 #include "co_pdo.h"
 #include "co_sync.h"
 #include "co_emcy.h"
+#include "co_od.h"
 #include "co_heartbeat.h"
 #include "co_node_guard.h"
 #include "co_lss.h"
@@ -143,6 +144,10 @@ void co_main (void * arg)
       case CO_JOB_ERROR_GET:
          co_emcy_job (net, job);
          break;
+      case CO_JOB_LOCAL_READ:
+      case CO_JOB_LOCAL_WRITE:
+         co_od_job (net, job);
+         break;
       case CO_JOB_EXIT:
          running = false;
          break;
@@ -378,6 +383,56 @@ int co_error_get (co_client_t * client, uint8_t * error)
    return job->result;
 }
 
+int co_od_local_read (
+   co_client_t * client,
+   uint16_t index,
+   uint8_t subindex,
+   uint64_t * value)
+{
+   co_net_t * net = client->net;
+   co_job_t * job = &client->job;
+
+   job->client       = client;
+   job->od.index     = index;
+   job->od.subindex  = subindex;
+   job->callback     = co_job_callback;
+   job->type         = CO_JOB_LOCAL_READ;
+
+   os_mbox_post (net->mbox, job, OS_WAIT_FOREVER);
+   os_sem_wait (client->sem, OS_WAIT_FOREVER);
+
+   if (job->result == 0)
+   {
+      *value = job->od.value;
+   }
+
+   return job->result;
+}
+
+int co_od_local_write (
+   co_client_t * client,
+   uint16_t index,
+   uint8_t subindex,
+   uint64_t value,
+   bool pdo_event)
+{
+   co_net_t * net = client->net;
+   co_job_t * job = &client->job;
+
+   job->client       = client;
+   job->od.index     = index;
+   job->od.subindex  = subindex;
+   job->od.value     = value;
+   job->od.pdo_event = pdo_event;
+   job->callback     = co_job_callback;
+   job->type         = CO_JOB_LOCAL_WRITE;
+
+   os_mbox_post (net->mbox, job, OS_WAIT_FOREVER);
+   os_sem_wait (client->sem, OS_WAIT_FOREVER);
+
+   return job->result;
+}
+
 co_client_t * co_client_init (co_net_t * net)
 {
    co_client_t * client;

src/co_main.h

@@ -103,6 +103,8 @@ typedef enum co_job_type
    CO_JOB_ERROR_SET,
    CO_JOB_ERROR_CLEAR,
    CO_JOB_ERROR_GET,
+   CO_JOB_LOCAL_READ,
+   CO_JOB_LOCAL_WRITE,
    CO_JOB_EXIT,
 } co_job_type_t;
 
@@ -139,6 +141,17 @@ typedef struct co_pdo_job
    uint8_t subindex;
 } co_pdo_job_t;
 
+typedef struct co_od_job
+{
+   uint16_t index;
+   uint8_t subindex;
+   uint64_t value;
+   struct
+   {
+      bool pdo_event : 1;
+   };
+} co_od_job_t;
+
 /** Generic job */
 typedef struct co_job
 {
@@ -148,6 +161,7 @@ typedef struct co_job
       co_sdo_job_t sdo;
       co_emcy_job_t emcy;
       co_pdo_job_t pdo;
+      co_od_job_t od;
    };
    uint32_t timestamp;
    struct co_client * client;

src/co_od.c

@@ -18,6 +18,7 @@
 
 #include "co_od.h"
 #include "co_sdo.h"
+#include "co_pdo.h"
 #include "co_util.h"
 
 #include <string.h>
@@ -266,6 +267,78 @@ uint32_t co_od_set_value (
    return 0;
 }
 
+static uint32_t co_od_set_by_index (
+   co_net_t * net,
+   uint16_t index,
+   uint8_t subindex,
+   uint64_t value)
+{
+   const co_obj_t * obj;
+   const co_entry_t * entry;
+
+   obj = co_obj_find (net, index);
+   if (obj == NULL)
+      return CO_SDO_ABORT_BAD_INDEX;
+
+   entry = co_entry_find (net, obj, subindex);
+   if (entry == NULL)
+      return CO_SDO_ABORT_BAD_SUBINDEX;
+
+   if (CO_BYTELENGTH (entry->bitlength) > sizeof (value))
+      return CO_SDO_ABORT_LENGTH;
+
+   return co_od_set_value (net, obj, entry, subindex, value);
+}
+
+static uint32_t co_od_get_by_index (
+   co_net_t * net,
+   uint16_t index,
+   uint8_t subindex,
+   uint64_t * value)
+{
+   const co_obj_t * obj;
+   const co_entry_t * entry;
+
+   obj = co_obj_find (net, index);
+   if (obj == NULL)
+      return CO_SDO_ABORT_BAD_INDEX;
+
+   entry = co_entry_find (net, obj, subindex);
+   if (entry == NULL)
+      return CO_SDO_ABORT_BAD_SUBINDEX;
+
+   if (CO_BYTELENGTH (entry->bitlength) > sizeof (* value))
+      return CO_SDO_ABORT_LENGTH;
+
+   return co_od_get_value (net, obj, entry, subindex, value);
+}
+
+void co_od_job (co_net_t * net, co_job_t * job)
+{
+   uint32_t abort = 0;
+
+   switch (job->type)
+   {
+   case CO_JOB_LOCAL_READ:
+      abort = co_od_get_by_index (net, job->od.index, job->od.subindex, &job->od.value);
+      break;
+   case CO_JOB_LOCAL_WRITE:
+      abort = co_od_set_by_index (net, job->od.index, job->od.subindex, job->od.value);
+
+      if (abort == 0 && job->od.pdo_event)
+      {
+         co_pdo_trigger_with_obj(net, job->od.index, job->od.subindex);
+      }
+      break;
+   default:
+      CC_ASSERT (0);
+   }
+
+   job->result = abort == 0 ? 0 : CO_STATUS_ERROR;
+   if (job->callback)
+      job->callback (job);
+}
+
 void co_od_set_defaults (co_net_t * net, uint16_t min, uint16_t max)
 {
    const co_default_t * item = net->defaults;

src/co_od.h

@@ -271,6 +271,14 @@ uint32_t co_od_set_value (
    uint8_t subindex,
    uint64_t value);
 
+/**
+ * Perform local OD job
+ *
+ * @param net           network handle
+ * @param job           emcy job
+ */
+void co_od_job (co_net_t * net, co_job_t * job);
+
 #ifdef __cplusplus
 }
 #endif