From 69b104997098d5dbbf23496fda103a47e8f96e6a Mon Sep 17 00:00:00 2001
From: Lars Danielsson <lars.danielsson@rt-labs.com>
Date: Mon, 9 May 2022 09:27:07 +0200
Subject: [PATCH] Add support for Explicit Device Identification

Change-Id: Idbf636f37c02923d251742cc5f49ff1579eaebc6
---
 soes/esc.c                       | 80 +++++++++++++++++++++++++++++++-
 soes/esc.h                       |  6 +++
 soes/hal/rt-kernel-xmc4/esc_hw.c |  2 +-
 soes/hal/rt-kernel-xmc4/esc_hw.h |  2 +-
 4 files changed, 87 insertions(+), 3 deletions(-)

diff --git a/soes/esc.c b/soes/esc.c
index 582c1e0..df77643 100644
--- a/soes/esc.c
+++ b/soes/esc.c
@@ -920,6 +920,75 @@ void ESC_sm_act_event (void)
       ESC_SMack (7);
    }
 }
+
+static bool ESC_check_id_request (uint16_t ALcontrol, uint8_t * an)
+{
+   if ((ALcontrol & ESCREG_AL_ID_REQUEST) != 0)
+   {
+      uint8_t state = ALcontrol & ESCREG_AL_ERRACKMASK;
+
+      if ((state != ESCboot) &&
+          ((state < ESCsafeop) || (*an == ESCsafeop) || (*an == ESCop)))
+      {
+         uint16_t ALstatuscode;
+
+         ESC_read (ESCREG_ALERROR,
+                   (void *)&ALstatuscode,
+                   sizeof (ALstatuscode));
+
+         return (ALstatuscode == ALERR_NONE);
+      }
+   }
+
+   return false;
+}
+
+static uint8_t ESC_load_device_id (void)
+{
+   uint16_t device_id;
+
+   if (ESCvar.get_device_id != NULL)
+   {
+      if (ESCvar.get_device_id (&device_id) != 0)
+      {
+         device_id = 0;
+      }
+   }
+   else
+   {
+      ESC_read (ESCREG_CONF_STATION_ALIAS,
+                (void *)&device_id,
+                sizeof (device_id));
+   }
+
+   if (device_id != 0)
+   {
+      /* Load the Device Identification Value to the AL Status Code register */
+      ESC_ALerror (device_id);
+
+      return ESCREG_AL_ID_REQUEST;
+   }
+
+   return 0;
+}
+
+#ifdef ESC_DEBUG
+static char * ESC_state_to_string (uint8_t ESC_state)
+{
+   switch (ESC_state)
+   {
+      case ESCinit:   return "Init";
+      case ESCpreop:  return "Pre-Operational";
+      case ESCboot:   return "Bootstrap";
+      case ESCsafeop: return "Safe-Operational";
+      case ESCop:     return "Operational";
+      case ESCerror:  return "Error";
+   }
+
+   return "Unknown";
+}
+#endif
+
 /** The state handler acting on ALControl Bit(0)
  * events in the Al Event Request register 0x220.
  *
@@ -1135,8 +1204,16 @@ void ESC_state (void)
       ESC_ALerror (ALERR_NONE);
    }
 
+   if (ESC_check_id_request (ESCvar.ALcontrol, &an))
+   {
+      an |= ESC_load_device_id ();
+   }
+
    ESC_ALstatus (an);
-   DPRINT ("state %x\n", an);
+
+#ifdef ESC_DEBUG
+   DPRINT ("state %s\n", ESC_state_to_string (an & 0xF));
+#endif
 }
 /** Function copying the application configuration variable
  * data to the stack local variable.
@@ -1176,4 +1253,5 @@ void ESC_config (esc_cfg_t * cfg)
    ESCvar.esc_hw_interrupt_disable = cfg->esc_hw_interrupt_disable;
    ESCvar.esc_hw_eep_handler = cfg->esc_hw_eep_handler;
    ESCvar.esc_check_dc_handler = cfg->esc_check_dc_handler;
+   ESCvar.get_device_id = cfg->get_device_id;
 }
diff --git a/soes/esc.h b/soes/esc.h
index d3beed0..d06aa1e 100644
--- a/soes/esc.h
+++ b/soes/esc.h
@@ -17,9 +17,12 @@
 #include "options.h"
 
 #define ESCREG_ADDRESS              0x0010
+#define ESCREG_CONF_STATION_ALIAS   0x0012
 #define ESCREG_DLSTATUS             0x0110
 #define ESCREG_ALCONTROL            0x0120
+#define ESCREG_ALCONTROL_ERROR_ACK  0x0010
 #define ESCREG_ALSTATUS             0x0130
+#define ESCREG_ALSTATUS_ERROR_IND   0x0010
 #define ESCREG_ALERROR              0x0134
 #define ESCREG_ALEVENTMASK          0x0204
 #define ESCREG_ALEVENT              0x0220
@@ -58,6 +61,7 @@
 #define ESCREG_AL_STATEMASK         0x001f
 #define ESCREG_AL_ALLBUTINITMASK    0x0e
 #define ESCREG_AL_ERRACKMASK        0x0f
+#define ESCREG_AL_ID_REQUEST        0x0020
 
 #define SYNCTYPE_SUPPORT_FREERUN    0x01
 #define SYNCTYPE_SUPPORT_SYNCHRON   0x02
@@ -336,6 +340,7 @@ typedef struct esc_cfg
    void (*esc_hw_interrupt_disable) (uint32_t mask);
    void (*esc_hw_eep_handler) (void);
    uint16_t (*esc_check_dc_handler) (void);
+   int (*get_device_id) (uint16_t * device_id);
 } esc_cfg_t;
 
 typedef struct
@@ -460,6 +465,7 @@ typedef struct
    void (*esc_hw_interrupt_disable) (uint32_t mask);
    void (*esc_hw_eep_handler) (void);
    uint16_t (*esc_check_dc_handler) (void);
+   int (*get_device_id) (uint16_t * device_id);
    uint8_t MBXrun;
    size_t activembxsize;
    sm_cfg_t * activemb0;
diff --git a/soes/hal/rt-kernel-xmc4/esc_hw.c b/soes/hal/rt-kernel-xmc4/esc_hw.c
index 0d76d4a..bf36838 100644
--- a/soes/hal/rt-kernel-xmc4/esc_hw.c
+++ b/soes/hal/rt-kernel-xmc4/esc_hw.c
@@ -10,7 +10,7 @@
  * Function to read and write commands to the ESC. Used to read/write ESC
  * registers and memory.
  */
-#include <kern.h>
+#include <kern/kern.h>
 #include <bsp.h>
 #include <xmc4.h>
 #include <eru.h>
diff --git a/soes/hal/rt-kernel-xmc4/esc_hw.h b/soes/hal/rt-kernel-xmc4/esc_hw.h
index ad61566..fe1914d 100644
--- a/soes/hal/rt-kernel-xmc4/esc_hw.h
+++ b/soes/hal/rt-kernel-xmc4/esc_hw.h
@@ -11,7 +11,7 @@
 #ifndef __esc_hw__
 #define __esc_hw__
 
-#include <kern.h>
+#include <kern/kern.h>
 /* ================================================================================ */
 /* ================                  ECAT [ECAT0]                  ================ */
 /* ================================================================================ */