From bf829cd7923b53b19268cbe749162e8fc2c5e987 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 12 Oct 2018 10:35:04 +1100
Subject: [PATCH] AP_Notify: use WITH_SEMAPHORE()

and removed usage of hal.util->new_semaphore()
---
 libraries/AP_Notify/ToneAlarm.cpp      | 45 ++++++++++++--------------
 libraries/AP_Notify/ToneAlarm.h        |  2 +-
 libraries/AP_Notify/ToshibaLED_I2C.cpp |  8 ++---
 3 files changed, 24 insertions(+), 31 deletions(-)

diff --git a/libraries/AP_Notify/ToneAlarm.cpp b/libraries/AP_Notify/ToneAlarm.cpp
index bcc51fca5a..e1daf1f2d1 100644
--- a/libraries/AP_Notify/ToneAlarm.cpp
+++ b/libraries/AP_Notify/ToneAlarm.cpp
@@ -18,6 +18,7 @@
 
 #include <AP_HAL/AP_HAL.h>
 #include <AP_Math/AP_Math.h>
+#include <AP_Common/Semaphore.h>
 
 #include "ToneAlarm.h"
 #include "AP_Notify.h"
@@ -100,8 +101,6 @@ bool AP_ToneAlarm::init()
         return false;
     }
 
-    _sem = hal.util->new_semaphore();
-
     // set initial boot states. This prevents us issuing a arming
     // warning in plane and rover on every boot
     flags.armed = AP_Notify::flags.armed;
@@ -131,21 +130,18 @@ void AP_ToneAlarm::play_tone(const uint8_t tone_index)
 
 void AP_ToneAlarm::_timer_task()
 {
-    if (_sem && _sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
-        _mml_player.update();
-        _sem->give();
-    }
+    WITH_SEMAPHORE(_sem);
+    _mml_player.update();
 }
 
 void AP_ToneAlarm::play_string(const char *str)
 {
-    if (_sem && _sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
-        _mml_player.stop();
-        strncpy(_tone_buf, str, AP_NOTIFY_TONEALARM_TONE_BUF_SIZE);
-        _tone_buf[AP_NOTIFY_TONEALARM_TONE_BUF_SIZE-1] = 0;
-        _mml_player.play(_tone_buf);
-        _sem->give();
-    }
+    WITH_SEMAPHORE(_sem);
+
+    _mml_player.stop();
+    strncpy(_tone_buf, str, AP_NOTIFY_TONEALARM_TONE_BUF_SIZE);
+    _tone_buf[AP_NOTIFY_TONEALARM_TONE_BUF_SIZE-1] = 0;
+    _mml_player.play(_tone_buf);
 }
 
 void AP_ToneAlarm::stop_cont_tone()
@@ -390,17 +386,16 @@ void AP_ToneAlarm::handle_play_tune(mavlink_message_t *msg)
 
     mavlink_msg_play_tune_decode(msg, &packet);
 
-    if (_sem && _sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
-        _mml_player.stop();
+    WITH_SEMAPHORE(_sem);
 
-        strncpy(_tone_buf, packet.tune, MIN(sizeof(packet.tune), sizeof(_tone_buf)-1));
-        _tone_buf[sizeof(_tone_buf)-1] = 0;
-        uint8_t len = strlen(_tone_buf);
-        uint8_t len2 = strnlen(packet.tune2, sizeof(packet.tune2));
-        len2 = MIN((sizeof(_tone_buf)-1)-len, len2);
-        strncpy(_tone_buf+len, packet.tune2, len2);
-        _tone_buf[sizeof(_tone_buf)-1] = 0;
-        _mml_player.play(_tone_buf);
-        _sem->give();
-    }
+    _mml_player.stop();
+
+    strncpy(_tone_buf, packet.tune, MIN(sizeof(packet.tune), sizeof(_tone_buf)-1));
+    _tone_buf[sizeof(_tone_buf)-1] = 0;
+    uint8_t len = strlen(_tone_buf);
+    uint8_t len2 = strnlen(packet.tune2, sizeof(packet.tune2));
+    len2 = MIN((sizeof(_tone_buf)-1)-len, len2);
+    strncpy(_tone_buf+len, packet.tune2, len2);
+    _tone_buf[sizeof(_tone_buf)-1] = 0;
+    _mml_player.play(_tone_buf);
 }
diff --git a/libraries/AP_Notify/ToneAlarm.h b/libraries/AP_Notify/ToneAlarm.h
index f8f41db6ad..58c6d1935a 100644
--- a/libraries/AP_Notify/ToneAlarm.h
+++ b/libraries/AP_Notify/ToneAlarm.h
@@ -77,7 +77,7 @@ private:
 
     const static Tone _tones[];
 
-    AP_HAL::Semaphore* _sem;
+    HAL_Semaphore _sem;
     MMLPlayer _mml_player;
     char _tone_buf[AP_NOTIFY_TONEALARM_TONE_BUF_SIZE];
 };
diff --git a/libraries/AP_Notify/ToshibaLED_I2C.cpp b/libraries/AP_Notify/ToshibaLED_I2C.cpp
index 34f30d74cc..99afcb9deb 100644
--- a/libraries/AP_Notify/ToshibaLED_I2C.cpp
+++ b/libraries/AP_Notify/ToshibaLED_I2C.cpp
@@ -23,6 +23,7 @@
 #include <utility>
 
 #include <AP_HAL/AP_HAL.h>
+#include <AP_Common/Semaphore.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -48,16 +49,16 @@ bool ToshibaLED_I2C::hw_init(void)
 {
     // first look for led on external bus
     _dev = std::move(hal.i2c_mgr->get_device(_bus, TOSHIBA_LED_I2C_ADDR));
-    if (!_dev || !_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
+    if (!_dev) {
         return false;
     }
+    WITH_SEMAPHORE(_dev->get_semaphore());
 
     _dev->set_retries(10);
 
     // enable the led
     bool ret = _dev->write_register(TOSHIBA_LED_ENABLE, 0x03);
     if (!ret) {
-        _dev->get_semaphore()->give();
         return false;
     }
 
@@ -67,9 +68,6 @@ bool ToshibaLED_I2C::hw_init(void)
 
     _dev->set_retries(1);
 
-    // give back i2c semaphore
-    _dev->get_semaphore()->give();
-
     if (ret) {
         _dev->register_periodic_callback(20000, FUNCTOR_BIND_MEMBER(&ToshibaLED_I2C::_timer, void));
     }