AP_HAL_SITL: add bus ownership check for simulated i2c transfers

Also stop copying the bus object while iterating

libraries/AP_HAL_SITL/I2CDevice.cpp

@@ -29,6 +29,9 @@ using namespace HALSITL;
 class I2CBus {
     friend class I2CDeviceManager;
 public:
+
+    I2CBus() { bus = i2c_buscount++; }
+
     uint8_t bus;
     Semaphore sem;
     int ioctl(uint8_t ioctl_number, void *data) {
@@ -46,8 +49,13 @@ private:
         uint32_t period_usec;
         uint64_t next_usec;
     } *callbacks;
+
+    static uint8_t i2c_buscount;
+
 };
 
+uint8_t I2CBus::i2c_buscount;
+
 int I2CBus::_ioctl(uint8_t ioctl_number, void *data)
 {
     SITL::SITL *sitl = AP::sitl();
@@ -76,6 +84,7 @@ void I2CBus::_timer_tick()
     const uint64_t now = AP_HAL::micros64();
     for (struct callback_info *ci = callbacks; ci != nullptr; ci = ci->next) {
         if (ci->next_usec >= now) {
+            WITH_SEMAPHORE(sem);
             ci->cb();
             ci->next_usec += ci->period_usec;
         }
@@ -86,7 +95,7 @@ void I2CBus::_timer_tick()
  * I2CDeviceManager
  */
 
-I2CBus I2CDeviceManager::buses[NUM_SITL_I2C_BUSES];
+I2CBus I2CDeviceManager::buses[NUM_SITL_I2C_BUSES] {};
 
 I2CDeviceManager::I2CDeviceManager()
 {
@@ -108,7 +117,7 @@ I2CDeviceManager::get_device(uint8_t bus,
 
 void I2CDeviceManager::_timer_tick()
 {
-    for (auto bus : buses) {
+    for (auto &bus : buses) {
         bus._timer_tick();
     }
 }
@@ -132,7 +141,8 @@ I2CDevice::I2CDevice(I2CBus &bus, uint8_t address)
 bool I2CDevice::transfer(const uint8_t *send, uint32_t send_len,
                          uint8_t *recv, uint32_t recv_len)
 {
-//    kill(0, SIGTRAP);
+    _bus.sem.check_owner();
+
     // combined transfer
     if (!_transfer(send, send_len, recv, recv_len)) {
         return false;

libraries/AP_HAL_SITL/Semaphores.cpp

@@ -24,15 +24,29 @@ bool Semaphore::give()
     if (pthread_mutex_unlock(&_lock) != 0) {
         AP_HAL::panic("Bad semaphore usage");
     }
+    owner = (pthread_t)-1;
     return true;
 }
 
+void Semaphore::check_owner()
+{
+    // should probably make sure we're holding the semaphore here....
+    if (owner != pthread_self()) {
+        AP_HAL::panic("Wrong owner");
+    }
+}
+
 bool Semaphore::take(uint32_t timeout_ms)
 {
     if (timeout_ms == HAL_SEMAPHORE_BLOCK_FOREVER) {
-        return pthread_mutex_lock(&_lock) == 0;
+        if (pthread_mutex_lock(&_lock) == 0) {
+            owner = pthread_self();
+            return true;
+        }
+        return false;
     }
     if (take_nonblocking()) {
+        owner = pthread_self();
         return true;
     }
     uint64_t start = AP_HAL::micros64();
@@ -41,6 +55,7 @@ bool Semaphore::take(uint32_t timeout_ms)
         hal.scheduler->delay_microseconds(200);
         Scheduler::from(hal.scheduler)->set_in_semaphore_take_wait(false);
         if (take_nonblocking()) {
+            owner = pthread_self();
             return true;
         }
     } while ((AP_HAL::micros64() - start) < timeout_ms * 1000);

libraries/AP_HAL_SITL/Semaphores.h

@@ -13,6 +13,11 @@ public:
     bool give() override;
     bool take(uint32_t timeout_ms) override;
     bool take_nonblocking() override;
+
+    void check_owner();  // asserts that current thread owns semaphore
+
 protected:
     pthread_mutex_t _lock;
+    pthread_t owner;
+
 };