MLBEDSW-2447 Set clock_q_enable & power_q_enable correctly

Bit [2] in CMD register is used to enable clock off using
clock q-interface and enable the master clock gate. Bit [3] is
used to enable power off using power q-interface.

The clock_q_enable bit is set when putting the Ethos-U into run
state.

The power_q_enable bit is set to 0 when running the command stream
and set to 1 after command stream has finished running.

Change-Id: Id9d1405376508e2af2ec0ddc2ebae8fb6c2f5cba

2 files changed, 52 insertions, 15 deletions

diff --git a/src/ethosu_device.c b/src/ethosu_device.c
index 7df8b6f..cc8ca12 100644
--- a/src/ethosu_device.c
+++ b/src/ethosu_device.c
@@ -80,6 +80,8 @@ enum ethosu_error_codes ethosu_run_command_stream(const uint8_t *cmd_stream_ptr,
                                                   const uint64_t *base_addr,
                                                   int num_base_addr)
 {
+    enum ethosu_error_codes ret_code = ETHOSU_SUCCESS;
+
 #if !defined(ARM_NPU_STUB)
     uint32_t qbase0;
     uint32_t qbase1;
@@ -103,7 +105,8 @@ enum ethosu_error_codes ethosu_run_command_stream(const uint8_t *cmd_stream_ptr,
     write_reg(NPU_REG_QBASE0, qbase0);
     write_reg(NPU_REG_QBASE1, qbase1);
     write_reg(NPU_REG_QSIZE, qsize);
-    write_reg(NPU_REG_CMD, 1);
+
+    ret_code = ethosu_set_command_run();
 #else
     // NPU stubbed
     stream_length = cms_length;
@@ -115,7 +118,7 @@ enum ethosu_error_codes ethosu_run_command_stream(const uint8_t *cmd_stream_ptr,
 #endif
 #endif
 
-    return ETHOSU_SUCCESS;
+    return ret_code;
 }
 
 enum ethosu_error_codes ethosu_is_irq_raised(uint8_t *irq_raised)
@@ -140,13 +143,21 @@ enum ethosu_error_codes ethosu_is_irq_raised(uint8_t *irq_raised)
 enum ethosu_error_codes ethosu_clear_irq_status(void)
 {
 #if !defined(ARM_NPU_STUB)
-    write_reg(NPU_REG_CMD, 2);
+    struct cmd_r oldcmd;
+    oldcmd.word = read_reg(NPU_REG_CMD);
+
+    struct cmd_r cmd;
+    cmd.word           = 0;
+    cmd.clear_irq      = 1;
+    cmd.clock_q_enable = oldcmd.clock_q_enable;
+    cmd.power_q_enable = oldcmd.power_q_enable;
+    write_reg(NPU_REG_CMD, cmd.word);
 #else
 #endif
     return ETHOSU_SUCCESS;
 }
 
-// TODO Understand settings of privilege/sequrity level and update API.
+// TODO Understand settings of privilege/security level and update API.
 enum ethosu_error_codes ethosu_soft_reset(void)
 {
     enum ethosu_error_codes return_code = ETHOSU_SUCCESS;
@@ -400,7 +411,15 @@ enum ethosu_error_codes ethosu_get_irq_history_mask(uint16_t *irq_history_mask)
 enum ethosu_error_codes ethosu_clear_irq_history_mask(uint16_t irq_history_clear_mask)
 {
 #if !defined(ARM_NPU_STUB)
-    write_reg(NPU_REG_CMD, (uint32_t)irq_history_clear_mask << 16);
+    struct cmd_r oldcmd;
+    oldcmd.word = read_reg(NPU_REG_CMD);
+
+    struct cmd_r cmd;
+    cmd.word              = 0;
+    cmd.clock_q_enable    = oldcmd.clock_q_enable;
+    cmd.power_q_enable    = oldcmd.power_q_enable;
+    cmd.clear_irq_history = irq_history_clear_mask;
+    write_reg(NPU_REG_CMD, cmd.word);
 #else
     UNUSED(irq_history_clear_mask);
 #endif
@@ -410,7 +429,15 @@ enum ethosu_error_codes ethosu_clear_irq_history_mask(uint16_t irq_history_clear
 enum ethosu_error_codes ethosu_set_command_run(void)
 {
 #if !defined(ARM_NPU_STUB)
-    write_reg(NPU_REG_CMD, 1);
+    struct cmd_r oldcmd;
+    oldcmd.word = read_reg(NPU_REG_CMD);
+
+    struct cmd_r cmd;
+    cmd.word                        = 0;
+    cmd.transition_to_running_state = 1;
+    cmd.clock_q_enable              = oldcmd.clock_q_enable;
+    cmd.power_q_enable              = oldcmd.power_q_enable;
+    write_reg(NPU_REG_CMD, cmd.word);
 #else
 #endif
     return ETHOSU_SUCCESS;
diff --git a/src/ethosu_driver.c b/src/ethosu_driver.c
index 56d8248..e372312 100644
--- a/src/ethosu_driver.c
+++ b/src/ethosu_driver.c
@@ -192,6 +192,12 @@ int ethosu_init(void)
     int return_code = 0;
     LOG_INFO("ethosu_init calling NPU embed driver ethosu_dev_init\n");
 
+    if (ETHOSU_SUCCESS != ethosu_set_clock_and_power(ETHOSU_CLOCK_Q_DISABLE, ETHOSU_POWER_Q_DISABLE))
+    {
+        LOG_ERR("Failed to disable clock-q & power-q for Ethos-U\n");
+        return -1;
+    }
+
     ethosu_soft_reset();
 
     if (ETHOSU_SUCCESS != ethosu_wait_for_reset())
@@ -264,15 +270,17 @@ int ethosu_invoke(const void *custom_data_ptr,
     }
     int custom_data_32bit_size = (custom_data_size / BYTES_IN_32_BITS - CUSTOM_OPTION_LENGTH_32_BIT_WORD);
 
+    ethosu_set_clock_and_power(ETHOSU_CLOCK_Q_ENABLE, ETHOSU_POWER_Q_DISABLE);
     while (data_ptr < (data_start_ptr + custom_data_32bit_size))
     {
+        int ret = 0;
         switch (data_ptr->driver_action_command)
         {
         case OPTIMIZER_CONFIG:
             LOG_INFO("ethosu_invoke OPTIMIZER_CONFIG\n");
             struct opt_cfg_s *opt_cfg_p = (struct opt_cfg_s *)data_ptr;
 
-            return_code = handle_optimizer_config(opt_cfg_p);
+            ret = handle_optimizer_config(opt_cfg_p);
             data_ptr += DRIVER_ACTION_LENGTH_32_BIT_WORD + OPTIMIZER_CONFIG_LENGTH_32_BIT_WORD;
             break;
         case COMMAND_STREAM:
@@ -284,9 +292,9 @@ int ethosu_invoke(const void *custom_data_ptr,
             // It is safe to clear this flag without atomic, because npu is not running.
             irq_triggered = false;
 
-            return_code = handle_command_stream(command_stream, cms_length, base_addr, num_base_addr);
+            ret = handle_command_stream(command_stream, cms_length, base_addr, num_base_addr);
 
-            if (return_code == -1 && abort_inference)
+            if (ret == -1 && abort_inference)
             {
                 uint32_t qread = 0;
                 ethosu_get_qread(&qread);
@@ -301,12 +309,12 @@ int ethosu_invoke(const void *custom_data_ptr,
             break;
         case READ_APB_REG:
             LOG_INFO("ethosu_invoke READ_APB_REG\n");
-            return_code = read_apb_reg(data_ptr->driver_action_data);
+            ret = read_apb_reg(data_ptr->driver_action_data);
             data_ptr += DRIVER_ACTION_LENGTH_32_BIT_WORD;
             break;
         case DUMP_SHRAM:
             LOG_INFO("ethosu_invoke DUMP_SHRAM\n");
-            return_code = dump_shram();
+            ret = dump_shram();
             data_ptr += DRIVER_ACTION_LENGTH_32_BIT_WORD;
             break;
         case NOP:
@@ -315,15 +323,17 @@ int ethosu_invoke(const void *custom_data_ptr,
             break;
         default:
             LOG_ERR("ethosu_invoke UNSUPPORTED driver_action_command %d \n", data_ptr->driver_action_command);
-            return -1;
+            ret = -1;
             break;
         }
-        if (return_code != 0)
+        if (ret != 0)
         {
-            return -1;
+            return_code = -1;
+            break;
         }
     }
-    return 0;
+    ethosu_set_clock_and_power(ETHOSU_CLOCK_Q_ENABLE, ETHOSU_POWER_Q_ENABLE);
+    return return_code;
 }
 
 void ethosu_abort(void)