Add mock HD programs to default AIDL bcradio HAL

Added mocked HD radio programs and HD radio tune/seek/step support with
subchannel in the default AIDL broadcast radio HAL implementiation.

Bug: 309694368
Test: atest VtsHalBroadcastradioAidlTargetTest
Change-Id: Id0ba9b127860d98822e9102ba48d913dc38cade1
diff --git a/broadcastradio/aidl/default/BroadcastRadio.cpp b/broadcastradio/aidl/default/BroadcastRadio.cpp
index 63073d7..c7e1134 100644
--- a/broadcastradio/aidl/default/BroadcastRadio.cpp
+++ b/broadcastradio/aidl/default/BroadcastRadio.cpp
@@ -81,10 +81,29 @@
 ProgramInfo makeSampleProgramInfo(const ProgramSelector& selector) {
     ProgramInfo info = {};
     info.selector = selector;
-    info.logicallyTunedTo =
-            utils::makeIdentifier(IdentifierType::AMFM_FREQUENCY_KHZ,
-                                  utils::getId(selector, IdentifierType::AMFM_FREQUENCY_KHZ));
-    info.physicallyTunedTo = info.logicallyTunedTo;
+    switch (info.selector.primaryId.type) {
+        case IdentifierType::AMFM_FREQUENCY_KHZ:
+            info.logicallyTunedTo = utils::makeIdentifier(
+                    IdentifierType::AMFM_FREQUENCY_KHZ,
+                    utils::getId(selector, IdentifierType::AMFM_FREQUENCY_KHZ));
+            info.physicallyTunedTo = info.logicallyTunedTo;
+            break;
+        case IdentifierType::HD_STATION_ID_EXT:
+            info.logicallyTunedTo = utils::makeIdentifier(IdentifierType::AMFM_FREQUENCY_KHZ,
+                                                          utils::getAmFmFrequency(info.selector));
+            info.physicallyTunedTo = info.logicallyTunedTo;
+            break;
+        case IdentifierType::DAB_SID_EXT:
+            info.logicallyTunedTo = info.selector.primaryId;
+            info.physicallyTunedTo = utils::makeIdentifier(
+                    IdentifierType::DAB_FREQUENCY_KHZ,
+                    utils::getId(selector, IdentifierType::DAB_FREQUENCY_KHZ));
+            break;
+        default:
+            info.logicallyTunedTo = info.selector.primaryId;
+            info.physicallyTunedTo = info.logicallyTunedTo;
+            break;
+    }
     return info;
 }
 
@@ -246,6 +265,99 @@
     return ScopedAStatus::ok();
 }
 
+bool BroadcastRadio::findNextLocked(const ProgramSelector& current, bool directionUp,
+                                    bool skipSubChannel, VirtualProgram* nextProgram) const {
+    if (mProgramList.empty()) {
+        return false;
+    }
+    // The list is not sorted here since it has already stored in VirtualRadio.
+    bool hasAmFmFrequency = utils::hasAmFmFrequency(current);
+    uint32_t currentFreq = hasAmFmFrequency ? utils::getAmFmFrequency(current) : 0;
+    auto found =
+            std::lower_bound(mProgramList.begin(), mProgramList.end(), VirtualProgram({current}));
+    if (directionUp) {
+        if (found < mProgramList.end() - 1) {
+            // When seeking up, tuner will jump to the first selector which is main program service
+            // greater than the current program selector in the program list (if not exist, jump
+            // to the first selector) for skipping sub-channels case or AM/FM without HD radio
+            // enabled case. Otherwise, the tuner will jump to the first selector greater than the
+            // current program selector.
+            if (utils::tunesTo(current, found->selector)) found++;
+            if (skipSubChannel && hasAmFmFrequency) {
+                auto firstFound = found;
+                while (utils::getAmFmFrequency(found->selector) == currentFreq) {
+                    if (found < mProgramList.end() - 1) {
+                        found++;
+                    } else {
+                        found = mProgramList.begin();
+                    }
+                    if (found == firstFound) {
+                        // Only one main channel exists in the program list, the tuner cannot skip
+                        // sub-channel to the next program selector.
+                        return false;
+                    }
+                }
+            }
+        } else {
+            // If the selector of current program is no less than all selectors or not found in the
+            // program list, seeking up should wrap the tuner to the beginning of the program list.
+            found = mProgramList.begin();
+        }
+    } else {
+        if (found > mProgramList.begin() && found != mProgramList.end()) {
+            // When seeking down, tuner will jump to the first selector which is main program
+            // service less than the current program selector in the program list (if not exist,
+            // jump to the last main program service selector) for skipping sub-channels case or
+            // AM/FM without HD radio enabled case. Otherwise, the tuner will jump to the first
+            // selector less than the current program selector.
+            found--;
+            if (hasAmFmFrequency && utils::hasAmFmFrequency(found->selector)) {
+                uint32_t nextFreq = utils::getAmFmFrequency(found->selector);
+                if (nextFreq != currentFreq) {
+                    jumpToFirstSubChannelLocked(found);
+                } else if (skipSubChannel) {
+                    jumpToFirstSubChannelLocked(found);
+                    auto firstFound = found;
+                    if (found > mProgramList.begin()) {
+                        found--;
+                    } else {
+                        found = mProgramList.end() - 1;
+                    }
+                    jumpToFirstSubChannelLocked(found);
+                    if (found == firstFound) {
+                        // Only one main channel exists in the program list, the tuner cannot skip
+                        // sub-channel to the next program selector.
+                        return false;
+                    }
+                }
+            }
+        } else {
+            // If the selector of current program is no greater than all selectors or not found
+            // in the program list, seeking down should wrap the tuner to the end of the program
+            // list. If the last program selector in the program list is sub-channel and skipping
+            // sub-channels is needed, the tuner will jump to the last main program service in
+            // the program list.
+            found = mProgramList.end() - 1;
+            jumpToFirstSubChannelLocked(found);
+        }
+    }
+    *nextProgram = *found;
+    return true;
+}
+
+void BroadcastRadio::jumpToFirstSubChannelLocked(vector<VirtualProgram>::const_iterator& it) const {
+    if (!utils::hasAmFmFrequency(it->selector) || it == mProgramList.begin()) {
+        return;
+    }
+    uint32_t currentFrequency = utils::getAmFmFrequency(it->selector);
+    it--;
+    while (it != mProgramList.begin() && utils::hasAmFmFrequency(it->selector) &&
+           utils::getAmFmFrequency(it->selector) == currentFrequency) {
+        it--;
+    }
+    it++;
+}
+
 ScopedAStatus BroadcastRadio::seek(bool directionUp, bool skipSubChannel) {
     LOG(DEBUG) << __func__ << ": seek " << (directionUp ? "up" : "down") << " with skipSubChannel? "
                << (skipSubChannel ? "yes" : "no") << "...";
@@ -259,11 +371,14 @@
 
     cancelLocked();
 
-    const auto& list = mVirtualRadio.getProgramList();
+    mProgramList = mVirtualRadio.getProgramList();
     std::shared_ptr<ITunerCallback> callback = mCallback;
     auto cancelTask = [callback]() { callback->onTuneFailed(Result::CANCELED, {}); };
-    if (list.empty()) {
-        mIsTuneCompleted = false;
+
+    VirtualProgram nextProgram = {};
+    bool foundNext = findNextLocked(mCurrentProgram, directionUp, skipSubChannel, &nextProgram);
+    mIsTuneCompleted = false;
+    if (!foundNext) {
         auto task = [callback]() {
             LOG(DEBUG) << "seek: program list is empty, seek couldn't stop";
 
@@ -274,31 +389,11 @@
         return ScopedAStatus::ok();
     }
 
-    // The list is not sorted here since it has already stored in VirtualRadio.
-    // If the list is not sorted in advance, it should be sorted here.
-    const auto& current = mCurrentProgram;
-    auto found = std::lower_bound(list.begin(), list.end(), VirtualProgram({current}));
-    if (directionUp) {
-        if (found < list.end() - 1) {
-            if (tunesTo(current, found->selector)) found++;
-        } else {
-            found = list.begin();
-        }
-    } else {
-        if (found > list.begin() && found != list.end()) {
-            found--;
-        } else {
-            found = list.end() - 1;
-        }
-    }
-    const ProgramSelector tuneTo = found->selector;
-
-    mIsTuneCompleted = false;
-    auto task = [this, tuneTo, callback]() {
+    auto task = [this, nextProgram, callback]() {
         ProgramInfo programInfo = {};
         {
             lock_guard<mutex> lk(mMutex);
-            programInfo = tuneInternalLocked(tuneTo);
+            programInfo = tuneInternalLocked(nextProgram.selector);
         }
         callback->onCurrentProgramInfoChanged(programInfo);
     };
@@ -319,13 +414,17 @@
 
     cancelLocked();
 
-    if (!utils::hasId(mCurrentProgram, IdentifierType::AMFM_FREQUENCY_KHZ)) {
+    int64_t stepTo;
+    if (utils::hasId(mCurrentProgram, IdentifierType::AMFM_FREQUENCY_KHZ)) {
+        stepTo = utils::getId(mCurrentProgram, IdentifierType::AMFM_FREQUENCY_KHZ);
+    } else if (mCurrentProgram.primaryId.type == IdentifierType::HD_STATION_ID_EXT) {
+        stepTo = utils::getHdFrequency(mCurrentProgram);
+    } else {
         LOG(WARNING) << __func__ << ": can't step in anything else than AM/FM";
         return ScopedAStatus::fromServiceSpecificErrorWithMessage(
                 resultToInt(Result::NOT_SUPPORTED), "cannot step in anything else than AM/FM");
     }
 
-    int64_t stepTo = utils::getId(mCurrentProgram, IdentifierType::AMFM_FREQUENCY_KHZ);
     std::optional<AmFmBandRange> range = getAmFmRangeLocked();
     if (!range) {
         LOG(ERROR) << __func__ << ": can't find current band or tune operation is in process";
@@ -473,12 +572,12 @@
         LOG(WARNING) << __func__ << ": tune operation is in process";
         return {};
     }
-    if (!utils::hasId(mCurrentProgram, IdentifierType::AMFM_FREQUENCY_KHZ)) {
+    if (!utils::hasAmFmFrequency(mCurrentProgram)) {
         LOG(WARNING) << __func__ << ": current program does not has AMFM_FREQUENCY_KHZ identifier";
         return {};
     }
 
-    int64_t freq = utils::getId(mCurrentProgram, IdentifierType::AMFM_FREQUENCY_KHZ);
+    int64_t freq = utils::getAmFmFrequency(mCurrentProgram);
     for (const auto& range : mAmFmConfig.ranges) {
         if (range.lowerBound <= freq && range.upperBound >= freq) {
             return range;
diff --git a/broadcastradio/aidl/default/BroadcastRadio.h b/broadcastradio/aidl/default/BroadcastRadio.h
index 0f818ce..4186bf1 100644
--- a/broadcastradio/aidl/default/BroadcastRadio.h
+++ b/broadcastradio/aidl/default/BroadcastRadio.h
@@ -39,21 +39,25 @@
   public:
     explicit BroadcastRadio(const VirtualRadio& virtualRadio);
     ~BroadcastRadio();
-    ndk::ScopedAStatus getAmFmRegionConfig(bool full, AmFmRegionConfig* returnConfigs) override;
+    ndk::ScopedAStatus getAmFmRegionConfig(bool full, AmFmRegionConfig* returnConfigs)
+            EXCLUDES(mMutex) override;
     ndk::ScopedAStatus getDabRegionConfig(std::vector<DabTableEntry>* returnConfigs) override;
     ndk::ScopedAStatus getImage(int32_t id, std::vector<uint8_t>* returnImage) override;
-    ndk::ScopedAStatus getProperties(Properties* returnProperties) override;
+    ndk::ScopedAStatus getProperties(Properties* returnProperties) EXCLUDES(mMutex) override;
 
-    ndk::ScopedAStatus setTunerCallback(const std::shared_ptr<ITunerCallback>& callback) override;
-    ndk::ScopedAStatus unsetTunerCallback() override;
-    ndk::ScopedAStatus tune(const ProgramSelector& program) override;
-    ndk::ScopedAStatus seek(bool directionUp, bool skipSubChannel) override;
-    ndk::ScopedAStatus step(bool directionUp) override;
-    ndk::ScopedAStatus cancel() override;
-    ndk::ScopedAStatus startProgramListUpdates(const ProgramFilter& filter) override;
-    ndk::ScopedAStatus stopProgramListUpdates() override;
-    ndk::ScopedAStatus isConfigFlagSet(ConfigFlag flag, bool* returnIsSet) override;
-    ndk::ScopedAStatus setConfigFlag(ConfigFlag flag, bool in_value) override;
+    ndk::ScopedAStatus setTunerCallback(const std::shared_ptr<ITunerCallback>& callback)
+            EXCLUDES(mMutex) override;
+    ndk::ScopedAStatus unsetTunerCallback() EXCLUDES(mMutex) override;
+    ndk::ScopedAStatus tune(const ProgramSelector& program) EXCLUDES(mMutex) override;
+    ndk::ScopedAStatus seek(bool directionUp, bool skipSubChannel) EXCLUDES(mMutex) override;
+    ndk::ScopedAStatus step(bool directionUp) EXCLUDES(mMutex) override;
+    ndk::ScopedAStatus cancel() EXCLUDES(mMutex) override;
+    ndk::ScopedAStatus startProgramListUpdates(const ProgramFilter& filter)
+            EXCLUDES(mMutex) override;
+    ndk::ScopedAStatus stopProgramListUpdates() EXCLUDES(mMutex) override;
+    ndk::ScopedAStatus isConfigFlagSet(ConfigFlag flag, bool* returnIsSet)
+            EXCLUDES(mMutex) override;
+    ndk::ScopedAStatus setConfigFlag(ConfigFlag flag, bool in_value) EXCLUDES(mMutex) override;
     ndk::ScopedAStatus setParameters(const std::vector<VendorKeyValue>& parameters,
                                      std::vector<VendorKeyValue>* returnParameters) override;
     ndk::ScopedAStatus getParameters(const std::vector<std::string>& keys,
@@ -62,7 +66,7 @@
             const std::shared_ptr<IAnnouncementListener>& listener,
             const std::vector<AnnouncementType>& enabled,
             std::shared_ptr<ICloseHandle>* returnCloseHandle) override;
-    binder_status_t dump(int fd, const char** args, uint32_t numArgs) override;
+    binder_status_t dump(int fd, const char** args, uint32_t numArgs) EXCLUDES(mMutex) override;
 
   private:
     const VirtualRadio& mVirtualRadio;
@@ -75,15 +79,20 @@
     bool mIsTuneCompleted GUARDED_BY(mMutex) = true;
     Properties mProperties GUARDED_BY(mMutex);
     ProgramSelector mCurrentProgram GUARDED_BY(mMutex) = {};
+    std::vector<VirtualProgram> mProgramList GUARDED_BY(mMutex) = {};
     std::shared_ptr<ITunerCallback> mCallback GUARDED_BY(mMutex);
 
     // Bitmap for all ConfigFlag values
     int mConfigFlagValues GUARDED_BY(mMutex) = 0;
 
-    std::optional<AmFmBandRange> getAmFmRangeLocked() const;
-    void cancelLocked();
-    ProgramInfo tuneInternalLocked(const ProgramSelector& sel);
-    void cancelProgramListUpdateLocked();
+    std::optional<AmFmBandRange> getAmFmRangeLocked() const REQUIRES(mMutex);
+    void cancelLocked() REQUIRES(mMutex);
+    ProgramInfo tuneInternalLocked(const ProgramSelector& sel) REQUIRES(mMutex);
+    void cancelProgramListUpdateLocked() REQUIRES(mMutex);
+    bool findNextLocked(const ProgramSelector& current, bool directionUp, bool skipSubChannel,
+                        VirtualProgram* nextProgram) const REQUIRES(mMutex);
+    void jumpToFirstSubChannelLocked(std::vector<VirtualProgram>::const_iterator& it) const
+            REQUIRES(mMutex);
 
     binder_status_t cmdHelp(int fd) const;
     binder_status_t cmdTune(int fd, const char** args, uint32_t numArgs);
@@ -93,7 +102,7 @@
     binder_status_t cmdStartProgramListUpdates(int fd, const char** args, uint32_t numArgs);
     binder_status_t cmdStopProgramListUpdates(int fd, uint32_t numArgs);
 
-    binder_status_t dumpsys(int fd);
+    binder_status_t dumpsys(int fd) EXCLUDES(mMutex);
 };
 
 }  // namespace aidl::android::hardware::broadcastradio
diff --git a/broadcastradio/aidl/default/VirtualProgram.cpp b/broadcastradio/aidl/default/VirtualProgram.cpp
index 4fe6567..19c1dcf 100644
--- a/broadcastradio/aidl/default/VirtualProgram.cpp
+++ b/broadcastradio/aidl/default/VirtualProgram.cpp
@@ -49,7 +49,12 @@
             break;
         case IdentifierType::HD_STATION_ID_EXT:
             info.logicallyTunedTo = selectId(IdentifierType::HD_STATION_ID_EXT);
-            info.physicallyTunedTo = selectId(IdentifierType::AMFM_FREQUENCY_KHZ);
+            if (utils::hasId(info.selector, IdentifierType::AMFM_FREQUENCY_KHZ)) {
+                info.physicallyTunedTo = selectId(IdentifierType::AMFM_FREQUENCY_KHZ);
+            } else {
+                info.physicallyTunedTo = utils::makeIdentifier(
+                        IdentifierType::AMFM_FREQUENCY_KHZ, utils::getHdFrequency(info.selector));
+            }
             break;
         case IdentifierType::DAB_SID_EXT:
             info.logicallyTunedTo = selectId(IdentifierType::DAB_SID_EXT);
@@ -91,9 +96,34 @@
     auto& l = lhs.selector;
     auto& r = rhs.selector;
 
-    // Two programs with the same primaryId are considered the same.
-    if (l.primaryId.type != r.primaryId.type) return l.primaryId.type < r.primaryId.type;
+    if ((utils::hasId(l, IdentifierType::AMFM_FREQUENCY_KHZ) ||
+         l.primaryId.type == IdentifierType::HD_STATION_ID_EXT) &&
+        (utils::hasId(r, IdentifierType::AMFM_FREQUENCY_KHZ) ||
+         r.primaryId.type == IdentifierType::HD_STATION_ID_EXT)) {
+        uint32_t freq1 = utils::getAmFmFrequency(l);
+        int subChannel1 = l.primaryId.type == IdentifierType::HD_STATION_ID_EXT
+                                  ? utils::getHdSubchannel(l)
+                                  : 0;
+        uint32_t freq2 = utils::getAmFmFrequency(r);
+        int subChannel2 = r.primaryId.type == IdentifierType::HD_STATION_ID_EXT
+                                  ? utils::getHdSubchannel(r)
+                                  : 0;
+        return freq1 < freq2 || (freq1 == freq2 && (l.primaryId.type < r.primaryId.type ||
+                                                    subChannel1 < subChannel2));
+    } else if (l.primaryId.type == IdentifierType::DAB_SID_EXT &&
+               l.primaryId.type == IdentifierType::DAB_SID_EXT) {
+        uint64_t dabFreq1 = utils::getId(l, IdentifierType::DAB_FREQUENCY_KHZ);
+        uint64_t dabFreq2 = utils::getId(r, IdentifierType::DAB_FREQUENCY_KHZ);
+        if (dabFreq1 != dabFreq2) {
+            return dabFreq1 < dabFreq2;
+        }
+        return utils::getId(l, IdentifierType::DAB_ENSEMBLE) <
+               utils::getId(r, IdentifierType::DAB_ENSEMBLE);
+    }
 
+    if (l.primaryId.type != r.primaryId.type) {
+        return l.primaryId.type < r.primaryId.type;
+    }
     return l.primaryId.value < r.primaryId.value;
 }
 
diff --git a/broadcastradio/aidl/default/VirtualRadio.cpp b/broadcastradio/aidl/default/VirtualRadio.cpp
index 86c5a96..ba43d8a 100644
--- a/broadcastradio/aidl/default/VirtualRadio.cpp
+++ b/broadcastradio/aidl/default/VirtualRadio.cpp
@@ -21,6 +21,7 @@
 
 using ::aidl::android::hardware::broadcastradio::utils::makeSelectorAmfm;
 using ::aidl::android::hardware::broadcastradio::utils::makeSelectorDab;
+using ::aidl::android::hardware::broadcastradio::utils::makeSelectorHd;
 using ::std::string;
 using ::std::vector;
 
@@ -38,11 +39,25 @@
 }
 
 bool VirtualRadio::getProgram(const ProgramSelector& selector, VirtualProgram* programOut) const {
-    for (const auto& program : mPrograms) {
-        if (utils::tunesTo(selector, program.selector)) {
-            *programOut = program;
-            return true;
+    for (auto it = mPrograms.begin(); it != mPrograms.end(); it++) {
+        if (!utils::tunesTo(selector, it->selector)) {
+            continue;
         }
+        auto firstMatchIt = it;
+        if (utils::hasAmFmFrequency(it->selector)) {
+            uint32_t channelFreq = utils::getAmFmFrequency(it->selector);
+            it++;
+            while (it != mPrograms.end() && utils::hasAmFmFrequency(it->selector) &&
+                   utils::getAmFmFrequency(it->selector) == channelFreq) {
+                if (it->selector == selector) {
+                    *programOut = *it;
+                    return true;
+                }
+                it++;
+            }
+        }
+        *programOut = *firstMatchIt;
+        return true;
     }
     return false;
 }
@@ -56,15 +71,27 @@
             {makeSelectorAmfm(/* frequency= */ 94900u), "Wild 94.9", "Drake ft. Rihanna",
                 "Too Good"},
             {makeSelectorAmfm(/* frequency= */ 96500u), "KOIT", "Celine Dion", "All By Myself"},
-            {makeSelectorAmfm(/* frequency= */ 97300u), "Alice@97.3", "Drops of Jupiter", "Train"},
-            {makeSelectorAmfm(/* frequency= */ 99700u), "99.7 Now!", "The Chainsmokers", "Closer"},
             {makeSelectorAmfm(/* frequency= */ 101300u), "101-3 KISS-FM", "Justin Timberlake",
                 "Rock Your Body"},
             {makeSelectorAmfm(/* frequency= */ 103700u), "iHeart80s @ 103.7", "Michael Jackson",
                 "Billie Jean"},
             {makeSelectorAmfm(/* frequency= */ 106100u), "106 KMEL", "Drake", "Marvins Room"},
-            {makeSelectorAmfm(/* frequency= */ 700u), "700 AM", "Artist700", "Title700"},
-            {makeSelectorAmfm(/* frequency= */ 1700u), "1700 AM", "Artist1700", "Title1700"},
+            {makeSelectorAmfm(/* frequency= */ 560u), "Talk Radio 560 KSFO", "Artist560", "Title560"},
+            {makeSelectorAmfm(/* frequency= */ 680u), "KNBR 680", "Artist680", "Title680"},
+            {makeSelectorAmfm(/* frequency= */ 97300u), "Alice@97.3", "Drops of Jupiter", "Train"},
+            {makeSelectorAmfm(/* frequency= */ 99700u), "99.7 Now!", "The Chainsmokers", "Closer"},
+            {makeSelectorHd(/* stationId= */ 0xA0000001u, /* subChannel= */ 0u, /* frequency= */ 97700u),
+                "K-LOVE", "ArtistHd0", "TitleHd0"},
+            {makeSelectorHd(/* stationId= */ 0xA0000001u, /* subChannel= */ 1u, /* frequency= */ 97700u),
+                "Air1", "ArtistHd1", "TitleHd1"},
+            {makeSelectorHd(/* stationId= */ 0xA0000001u, /* subChannel= */ 2u, /* frequency= */ 97700u),
+                "K-LOVE Classics", "ArtistHd2", "TitleHd2"},
+            {makeSelectorHd(/* stationId= */ 0xA0000001u, /* subChannel= */ 0u, /* frequency= */ 98500u),
+                "98.5-1 South Bay's Classic Rock", "ArtistHd0", "TitleHd0"},
+            {makeSelectorHd(/* stationId= */ 0xA0000001u, /* subChannel= */ 1u, /* frequency= */ 98500u),
+                "Highway 1 - Different", "ArtistHd1", "TitleHd1"},
+            {makeSelectorHd(/* stationId= */ 0xB0000001u, /* subChannel= */ 0u, /* frequency= */ 1170u),
+                "KLOK", "ArtistHd1", "TitleHd1"},
         });
     // clang-format on
     return amFmRadioMock;
diff --git a/broadcastradio/common/utilsaidl/include/broadcastradio-utils-aidl/Utils.h b/broadcastradio/common/utilsaidl/include/broadcastradio-utils-aidl/Utils.h
index b6fb33f..a139e00 100644
--- a/broadcastradio/common/utilsaidl/include/broadcastradio-utils-aidl/Utils.h
+++ b/broadcastradio/common/utilsaidl/include/broadcastradio-utils-aidl/Utils.h
@@ -139,6 +139,7 @@
 ProgramSelector makeSelectorAmfm(uint32_t frequency);
 ProgramSelector makeSelectorDab(uint64_t sidExt);
 ProgramSelector makeSelectorDab(uint64_t sidExt, uint32_t ensemble, uint64_t freq);
+ProgramSelector makeSelectorHd(uint64_t stationId, uint64_t subChannel, uint64_t frequency);
 
 bool satisfies(const ProgramFilter& filter, const ProgramSelector& sel);
 
@@ -158,6 +159,14 @@
 
 ProgramIdentifier makeHdRadioStationName(const std::string& name);
 
+uint32_t getHdFrequency(const ProgramSelector& sel);
+
+int getHdSubchannel(const ProgramSelector& sel);
+
+bool hasAmFmFrequency(const ProgramSelector& sel);
+
+uint32_t getAmFmFrequency(const ProgramSelector& sel);
+
 template <typename aidl_type>
 inline std::string vectorToString(const std::vector<aidl_type>& in_values) {
     return std::accumulate(std::begin(in_values), std::end(in_values), std::string{},
diff --git a/broadcastradio/common/utilsaidl/src/Utils.cpp b/broadcastradio/common/utilsaidl/src/Utils.cpp
index 2875318..4c99514 100644
--- a/broadcastradio/common/utilsaidl/src/Utils.cpp
+++ b/broadcastradio/common/utilsaidl/src/Utils.cpp
@@ -49,12 +49,6 @@
     return getId(a, type) == getId(b, type);
 }
 
-int getHdSubchannel(const ProgramSelector& sel) {
-    int64_t hdSidExt = getId(sel, IdentifierType::HD_STATION_ID_EXT, /* defaultValue */ 0);
-    hdSidExt >>= 32;        // Station ID number
-    return hdSidExt & 0xF;  // HD Radio subchannel
-}
-
 bool maybeGetId(const ProgramSelector& sel, const IdentifierType& type, int64_t* val) {
     // iterate through primaryId and secondaryIds
     for (auto it = begin(sel); it != end(sel); it++) {
@@ -132,8 +126,13 @@
         case IdentifierType::AMFM_FREQUENCY_KHZ:
             if (haveEqualIds(a, b, IdentifierType::HD_STATION_ID_EXT)) return true;
             if (haveEqualIds(a, b, IdentifierType::RDS_PI)) return true;
-            return getHdSubchannel(b) == 0 &&
-                   haveEqualIds(a, b, IdentifierType::AMFM_FREQUENCY_KHZ);
+            if (getHdSubchannel(b) != 0) {  // supplemental program services
+                return false;
+            }
+            return haveEqualIds(a, b, IdentifierType::AMFM_FREQUENCY_KHZ) ||
+                   (b.primaryId.type == IdentifierType::HD_STATION_ID_EXT &&
+                    static_cast<uint32_t>(getId(a, IdentifierType::AMFM_FREQUENCY_KHZ)) ==
+                            getAmFmFrequency(b));
         case IdentifierType::DAB_SID_EXT:
             if (!haveEqualIds(a, b, IdentifierType::DAB_SID_EXT)) {
                 return false;
@@ -316,6 +315,13 @@
     return sel;
 }
 
+ProgramSelector makeSelectorHd(uint64_t stationId, uint64_t subChannel, uint64_t frequency) {
+    ProgramSelector sel = {};
+    uint64_t sidExt = stationId | (subChannel << 32) | (frequency << 36);
+    sel.primaryId = makeIdentifier(IdentifierType::HD_STATION_ID_EXT, sidExt);
+    return sel;
+}
+
 ProgramSelector makeSelectorDab(uint64_t sidExt, uint32_t ensemble, uint64_t freq) {
     ProgramSelector sel = {};
     sel.primaryId = makeIdentifier(IdentifierType::DAB_SID_EXT, sidExt);
@@ -483,6 +489,32 @@
     return static_cast<IdentifierType>(typeAsInt);
 }
 
+int getHdSubchannel(const ProgramSelector& sel) {
+    int64_t hdSidExt = getId(sel, IdentifierType::HD_STATION_ID_EXT, kValueForNotFoundIdentifier);
+    hdSidExt >>= 32;        // Station ID number
+    return hdSidExt & 0xF;  // HD Radio subchannel
+}
+
+uint32_t getHdFrequency(const ProgramSelector& sel) {
+    int64_t hdSidExt = getId(sel, IdentifierType::HD_STATION_ID_EXT, kValueForNotFoundIdentifier);
+    if (hdSidExt == kValueForNotFoundIdentifier) {
+        return kValueForNotFoundIdentifier;
+    }
+    return static_cast<uint32_t>((hdSidExt >> 36) & 0x3FFFF);  // HD Radio subchannel
+}
+
+bool hasAmFmFrequency(const ProgramSelector& sel) {
+    return hasId(sel, IdentifierType::AMFM_FREQUENCY_KHZ) ||
+           sel.primaryId.type == IdentifierType::HD_STATION_ID_EXT;
+}
+
+uint32_t getAmFmFrequency(const ProgramSelector& sel) {
+    if (hasId(sel, IdentifierType::AMFM_FREQUENCY_KHZ)) {
+        return static_cast<uint32_t>(getId(sel, IdentifierType::AMFM_FREQUENCY_KHZ));
+    }
+    return getHdFrequency(sel);
+}
+
 bool parseArgInt(const std::string& s, int* out) {
     return ::android::base::ParseInt(s, out);
 }