Changed pru interface to support halting and restarting

src/motion-queue.h

@@ -148,6 +148,7 @@ class PRUMotionQueue final : public MotionQueue {
   bool Init();
 
   void ClearPRUAbort(unsigned int idx);
+  void ClearRingBuffer();
 
   HardwareMapping *const hardware_mapping_;
   PruHardwareInterface *const pru_interface_;

src/pru-hardware-interface.h

@@ -42,6 +42,13 @@ class PruHardwareInterface {
 
   // Halt the PRU
   virtual bool Shutdown() = 0;
+
+  // Halt any program execution. An halted program can be restarted.
+  virtual void Halt() = 0;
+
+  // Restart the execution of the previously halted program.
+  virtual void Restart() = 0;
+
 };
 
 class UioPrussInterface : public PruHardwareInterface {
@@ -51,6 +58,8 @@ class UioPrussInterface : public PruHardwareInterface {
   bool StartExecution() final;
   unsigned WaitEvent() final;
   bool Shutdown() final;
+  void Halt() final;
+  void Restart() final;
 };
 
 #endif  // BEAGLEG_PRU_HARDWARE_INTERFACE_

src/pru-motion-queue.cc

@@ -178,8 +178,12 @@ void PRUMotionQueue::Shutdown(bool flush_queue) {
 }
 
 bool PRUMotionQueue::Clear() {
-  pru_interface_->Shutdown();
-  return Init();
+  MotorEnable(false);
+  pru_interface_->Halt();
+  ClearRingBuffer();
+  queue_pos_ = 0;
+  pru_interface_->Restart();
+  return true;
 }
 
 PRUMotionQueue::~PRUMotionQueue() {}
@@ -192,18 +196,20 @@ PRUMotionQueue::PRUMotionQueue(HardwareMapping *hw, PruHardwareInterface *pru)
   assert(success);
 }
 
+void PRUMotionQueue::ClearRingBuffer() {
+  for (int i = 0; i < QUEUE_LEN; ++i) {
+    pru_data_->ring_buffer[i].state = STATE_EMPTY;
+  }
+}
+
 bool PRUMotionQueue::Init() {
   MotorEnable(false);  // motors off initially.
   if (!pru_interface_->Init()) return false;
 
   if (!pru_interface_->AllocateSharedMem((void **)&pru_data_,
                                          sizeof(*pru_data_)))
     return false;
-
-  for (int i = 0; i < QUEUE_LEN; ++i) {
-    pru_data_->ring_buffer[i].state = STATE_EMPTY;
-  }
+  ClearRingBuffer();
   queue_pos_ = 0;
-
   return pru_interface_->StartExecution();
 }

src/pru-motion-queue_test.cc

@@ -18,8 +18,6 @@
 #include "pru-hardware-interface.h"
 #include "segment-queue.h"
 
-using ::testing::NiceMock;
-
 // PRU-side mock implementation of the ring buffer.
 struct MockPRUCommunication {
   internal::QueueStatus status;
@@ -30,15 +28,16 @@ class MockPRUInterface : public PruHardwareInterface {
  public:
   MockPRUInterface() : execution_index_(QUEUE_LEN - 1) {
     mmap = NULL;
-    ON_CALL(*this, Init).WillByDefault([]() { return true; });
-    ON_CALL(*this, Shutdown).WillByDefault([]() { return true; });
   }
   ~MockPRUInterface() override { free(mmap); }
 
+  bool Init() final { return true; }
   bool StartExecution() final { return true; }
   unsigned WaitEvent() final { return 1; }
-  MOCK_METHOD(bool, Init, (), ());
-  MOCK_METHOD(bool, Shutdown, (), ());
+  bool Shutdown() final { return true; }
+
+  MOCK_METHOD(void, Halt, (), (final));
+  MOCK_METHOD(void, Restart, (), (final));
 
   bool AllocateSharedMem(void **pru_mmap, const size_t size) final {
     if (mmap != NULL) return true;
@@ -70,15 +69,15 @@ class MockPRUInterface : public PruHardwareInterface {
 
 TEST(PruMotionQueue, status_init) {
   MotorsRegister absolute_pos_loops;
-  NiceMock<MockPRUInterface> pru_interface;
+  MockPRUInterface pru_interface;
   HardwareMapping hmap = HardwareMapping();
   PRUMotionQueue motion_backend(&hmap, (PruHardwareInterface *)&pru_interface);
   EXPECT_EQ(motion_backend.GetPendingElements(NULL), 0);
 }
 
 TEST(PruMotionQueue, single_exec) {
   MotorsRegister absolute_pos_loops;
-  NiceMock<MockPRUInterface> pru_interface;
+  MockPRUInterface pru_interface;
   HardwareMapping hmap = HardwareMapping();
   PRUMotionQueue motion_backend(&hmap, (PruHardwareInterface *)&pru_interface);
 
@@ -91,7 +90,7 @@ TEST(PruMotionQueue, single_exec) {
 
 TEST(PruMotionQueue, full_exec) {
   MotorsRegister absolute_pos_loops;
-  NiceMock<MockPRUInterface> pru_interface;
+  MockPRUInterface pru_interface;
   HardwareMapping hmap = HardwareMapping();
   PRUMotionQueue motion_backend(&hmap, (PruHardwareInterface *)&pru_interface);
 
@@ -104,7 +103,7 @@ TEST(PruMotionQueue, full_exec) {
 
 TEST(PruMotionQueue, single_exec_some_loops) {
   MotorsRegister absolute_pos_loops;
-  NiceMock<MockPRUInterface> pru_interface;
+  MockPRUInterface pru_interface;
   HardwareMapping hmap = HardwareMapping();
   PRUMotionQueue motion_backend(&hmap, (PruHardwareInterface *)&pru_interface);
 
@@ -119,7 +118,7 @@ TEST(PruMotionQueue, single_exec_some_loops) {
 
 TEST(PruMotionQueue, one_round_queue) {
   MotorsRegister absolute_pos_loops;
-  NiceMock<MockPRUInterface> pru_interface;
+  MockPRUInterface pru_interface;
   HardwareMapping hmap = HardwareMapping();
   PRUMotionQueue motion_backend(&hmap, (PruHardwareInterface *)&pru_interface);
 
@@ -141,7 +140,7 @@ TEST(PruMotionQueue, one_round_queue) {
 // Check the PRU is reset and no elements are pending.
 TEST(PruMotionQueue, clear_queue) {
   MotorsRegister absolute_pos_loops;
-  NiceMock<MockPRUInterface> pru_interface;
+  MockPRUInterface pru_interface;
   HardwareMapping hmap = HardwareMapping();
   PRUMotionQueue motion_backend(&hmap, (PruHardwareInterface *)&pru_interface);
 
@@ -157,18 +156,18 @@ TEST(PruMotionQueue, clear_queue) {
   ASSERT_TRUE(testing::Mock::VerifyAndClearExpectations(&pru_interface));
   {
     testing::InSequence seq;
-    EXPECT_CALL(pru_interface, Shutdown())
-      .Times(1)
-      .WillRepeatedly(testing::Return(true));
-    EXPECT_CALL(pru_interface, Init()).Times(1).WillOnce(testing::Return(true));
+    EXPECT_CALL(pru_interface, Halt())
+      .Times(1);
+    EXPECT_CALL(pru_interface, Restart())
+      .Times(1);
   }
   EXPECT_TRUE(motion_backend.Clear());
   EXPECT_EQ(motion_backend.GetPendingElements(NULL), 0);
 }
 
 TEST(PruMotionQueue, exec_index_lt_queue_pos) {
   MotorsRegister absolute_pos_loops;
-  NiceMock<MockPRUInterface> pru_interface;
+  MockPRUInterface pru_interface;
   HardwareMapping hmap = HardwareMapping();
   PRUMotionQueue motion_backend(&hmap, (PruHardwareInterface *)&pru_interface);
 

src/uio-pruss-interface.cc

@@ -84,6 +84,14 @@ unsigned UioPrussInterface::WaitEvent() {
   return num_events;
 }
 
+void UioPrussInterface::Halt() {
+  prussdrv_pru_disable(PRU_NUM);
+}
+
+void UioPrussInterface::Restart() {
+  prussdrv_pru_enable_at(PRU_NUM, 0);
+}
+
 bool UioPrussInterface::Shutdown() {
   prussdrv_pru_disable(PRU_NUM);
   prussdrv_exit();