diff --git a/wpa_supplicant/aidl/mainline/sta_iface.cpp b/wpa_supplicant/aidl/mainline/sta_iface.cpp
index 4a858cb..08d2b04 100644
--- a/wpa_supplicant/aidl/mainline/sta_iface.cpp
+++ b/wpa_supplicant/aidl/mainline/sta_iface.cpp
@@ -41,18 +41,82 @@
     return ndk::ScopedAStatus::ok();
 }
 
-::ndk::ScopedAStatus StaIface::startUsdPublish(int32_t in_cmdId,
-        const UsdPublishConfig& in_publishConfig) {
+::ndk::ScopedAStatus StaIface::startUsdPublish(int32_t cmdId,
+        const UsdPublishConfig& publishConfig) {
+    if (!validateUsdPublishConfig(publishConfig)) {
+        wpa_printf(MSG_ERROR, "USD publish config is invalid");
+        return createStatus(SupplicantStatusCode::FAILURE_ARGS_INVALID);
+    }
+    wpabuf_unique_ptr ssiBuffer = {nullptr, nullptr};
+    if (!publishConfig.baseConfig.serviceSpecificInfo.empty()) {
+        ssiBuffer = convertVectorToWpaBuf(publishConfig.baseConfig.serviceSpecificInfo);
+        if (ssiBuffer.get() == nullptr) {
+            wpa_printf(MSG_ERROR, "Unable to convert USD publish SSI to buffer");
+            return createStatus(SupplicantStatusCode::FAILURE_UNKNOWN);
+        }
+    }
+
+    struct wpa_supplicant *wpa_s = retrieveIfacePtr();
+    struct nan_publish_params nanPublishParams =
+        convertAidlUsdPublishConfigToInternal(publishConfig);
+    int publishId = wpas_nan_usd_publish(
+        wpa_s, publishConfig.baseConfig.serviceName.c_str(),
+        convertAidlServiceProtoTypeToInternal(
+            publishConfig.baseConfig.serviceProtoType),
+        ssiBuffer.get(), &nanPublishParams, false /* p2p */);
+    // TODO: Return status code in a callback
     return ndk::ScopedAStatus::ok();
 }
 
-::ndk::ScopedAStatus StaIface::startUsdSubscribe(int32_t in_cmdId,
-        const UsdSubscribeConfig& in_subscribeConfig) {
+::ndk::ScopedAStatus StaIface::startUsdSubscribe(int32_t cmdId,
+        const UsdSubscribeConfig& subscribeConfig) {
+    if (!validateUsdSubscribeConfig(subscribeConfig)) {
+        wpa_printf(MSG_ERROR, "USD subscribe config is invalid");
+        return createStatus(SupplicantStatusCode::FAILURE_ARGS_INVALID);
+    }
+    wpabuf_unique_ptr ssiBuffer = {nullptr, nullptr};
+    if (!subscribeConfig.baseConfig.serviceSpecificInfo.empty()) {
+        ssiBuffer = convertVectorToWpaBuf(subscribeConfig.baseConfig.serviceSpecificInfo);
+        if (ssiBuffer.get() == nullptr) {
+            wpa_printf(MSG_ERROR, "Unable to convert USD subscribe SSI to buffer");
+            return createStatus(SupplicantStatusCode::FAILURE_UNKNOWN);
+        }
+    }
+
+    struct wpa_supplicant *wpa_s = retrieveIfacePtr();
+    struct nan_subscribe_params nanSubscribeParams =
+        convertAidlUsdSubscribeConfigToInternal(subscribeConfig);
+    int subscribeId = wpas_nan_usd_subscribe(
+        wpa_s, subscribeConfig.baseConfig.serviceName.c_str(),
+        convertAidlServiceProtoTypeToInternal(
+            subscribeConfig.baseConfig.serviceProtoType),
+        ssiBuffer.get(), &nanSubscribeParams, false /* p2p */);
+    // TODO: Return status code in a callback
     return ndk::ScopedAStatus::ok();
 }
 
-::ndk::ScopedAStatus StaIface::updateUsdPublish(int32_t in_publishId,
-        const std::vector<uint8_t>& in_serviceSpecificInfo) {
+::ndk::ScopedAStatus StaIface::updateUsdPublish(int32_t publishId,
+        const std::vector<uint8_t>& serviceSpecificInfo) {
+    if (!checkContainerSize(serviceSpecificInfo, kMaxUsdLocalSsiLengthBytes)) {
+        wpa_printf(MSG_ERROR, "Updated USD publish SSI of size %zu exceeds the"
+            " supported size of %d", serviceSpecificInfo.size(),
+            kMaxUsdLocalSsiLengthBytes);
+        return createStatus(SupplicantStatusCode::FAILURE_ARGS_INVALID);
+    }
+    wpabuf_unique_ptr ssiBuffer = {nullptr, nullptr};
+    if (!serviceSpecificInfo.empty()) {
+        ssiBuffer = convertVectorToWpaBuf(serviceSpecificInfo);
+        if (ssiBuffer.get() == nullptr) {
+            wpa_printf(MSG_ERROR, "Unable to convert updated USD publish SSI to buffer");
+            return createStatus(SupplicantStatusCode::FAILURE_UNKNOWN);
+        }
+    }
+    int status = wpas_nan_usd_update_publish(
+        retrieveIfacePtr(), publishId, ssiBuffer.get());
+    if (status < 0) {
+        wpa_printf(MSG_ERROR, "Failed to update USD publish");
+        return createStatus(SupplicantStatusCode::FAILURE_UNKNOWN);
+    }
     return ndk::ScopedAStatus::ok();
 }
 
@@ -68,6 +132,28 @@
     return ndk::ScopedAStatus::ok();
 }
 
-::ndk::ScopedAStatus StaIface::sendUsdMessage(const UsdMessageInfo& in_messageInfo) {
+::ndk::ScopedAStatus StaIface::sendUsdMessage(const UsdMessageInfo& messageInfo) {
+    if (!checkContainerSize(messageInfo.message, kMaxUsdLocalSsiLengthBytes)) {
+        wpa_printf(MSG_ERROR, "USD message of size %zu exceeds the supported size of %d",
+            messageInfo.message.size(), kMaxUsdLocalSsiLengthBytes);
+        return createStatus(SupplicantStatusCode::FAILURE_ARGS_INVALID);
+    }
+    wpabuf_unique_ptr msgBuffer = {nullptr, nullptr};
+    if (!messageInfo.message.empty()) {
+        msgBuffer = convertVectorToWpaBuf(messageInfo.message);
+        if (msgBuffer.get() == nullptr) {
+            wpa_printf(MSG_ERROR, "Unable to convert USD message contents to buffer");
+            return createStatus(SupplicantStatusCode::FAILURE_UNKNOWN);
+        }
+    }
+    int handle = messageInfo.ownId;
+    int reqInstanceId = messageInfo.peerId;
+    int status = wpas_nan_usd_transmit(
+        retrieveIfacePtr(), handle, msgBuffer.get(), nullptr /* elems */,
+        messageInfo.peerMacAddress.data(), reqInstanceId);
+    if (status < 0) {
+        wpa_printf(MSG_ERROR, "Failed to send USD message");
+        return createStatus(SupplicantStatusCode::FAILURE_UNKNOWN);
+    }
     return ndk::ScopedAStatus::ok();
 }
diff --git a/wpa_supplicant/aidl/mainline/usd_utils.h b/wpa_supplicant/aidl/mainline/usd_utils.h
index ee92da8..428a599 100644
--- a/wpa_supplicant/aidl/mainline/usd_utils.h
+++ b/wpa_supplicant/aidl/mainline/usd_utils.h
@@ -9,12 +9,20 @@
 #ifndef MAINLINE_SUPPLICANT_USD_UTILS_H
 #define MAINLINE_SUPPLICANT_USD_UTILS_H
 
+#include "utils.h"
+
+#include <aidl/android/system/wifi/mainline_supplicant/IStaInterface.h>
+#include <aidl/android/system/wifi/mainline_supplicant/UsdServiceProtoType.h>
+
 extern "C"
 {
 #include "utils/common.h"
 #include "src/common/nan_de.h"
 }
 
+using ::aidl::android::system::wifi::mainline_supplicant::IStaInterface;
+using ::aidl::android::system::wifi::mainline_supplicant::UsdServiceProtoType;
+
 constexpr bool kIsUsdPublisherSupported = true;
 constexpr bool kIsUsdSubscriberSupported = true;
 constexpr int32_t kMaxUsdLocalSsiLengthBytes = 1400;
@@ -23,6 +31,123 @@
 constexpr int32_t kMaxNumUsdPublishSessions = NAN_DE_MAX_SERVICE;
 constexpr int32_t kMaxNumUsdSubscribeSessions = NAN_DE_MAX_SERVICE;
 
-// TODO: Add helper functions here
+static bool validateUsdBaseConfig(IStaInterface::UsdBaseConfig baseConfig) {
+    if (!isValidEnumValue(baseConfig.serviceProtoType,
+            UsdServiceProtoType::GENERIC, UsdServiceProtoType::CSA_MATTER)) {
+        wpa_printf(MSG_ERROR, "Unknown protocol type received: %d",
+            static_cast<int>(baseConfig.serviceProtoType));
+        return false;
+    }
+    if (!checkContainerSize(baseConfig.serviceName, kMaxUsdServiceNameLengthBytes)) {
+        wpa_printf(MSG_ERROR, "Service name of size %zu exceeds the supported size of %d",
+            baseConfig.serviceName.size(), kMaxUsdServiceNameLengthBytes);
+        return false;
+    }
+    if (!checkContainerSize(baseConfig.serviceSpecificInfo, kMaxUsdLocalSsiLengthBytes)) {
+        wpa_printf(MSG_ERROR, "Service specific info of size %zu exceeds"
+            " the supported size of %d", baseConfig.serviceSpecificInfo.size(),
+            kMaxUsdLocalSsiLengthBytes);
+        return false;
+    }
+    if (baseConfig.txMatchFilter.has_value() && !checkContainerSize(
+            baseConfig.txMatchFilter.value(), kMaxUsdMatchFilterLengthBytes)) {
+        wpa_printf(MSG_ERROR, "TX match filter of size %zu exceeds"
+            " the supported size of %d", baseConfig.txMatchFilter.value().size(),
+            kMaxUsdMatchFilterLengthBytes);
+        return false;
+    }
+    if (baseConfig.rxMatchFilter.has_value() && !checkContainerSize(
+            baseConfig.rxMatchFilter.value(), kMaxUsdMatchFilterLengthBytes)) {
+        wpa_printf(MSG_ERROR, "RX match filter of size %zu exceeds"
+            " the supported size of %d", baseConfig.rxMatchFilter.value().size(),
+            kMaxUsdMatchFilterLengthBytes);
+        return false;
+    }
+    return true;
+}
+
+static bool validateUsdPublishConfig(IStaInterface::UsdPublishConfig publishConfig) {
+    if (!validateUsdBaseConfig(publishConfig.baseConfig)) {
+        return false;
+    }
+    if (!isValidEnumValue(publishConfig.publishType,
+            IStaInterface::UsdPublishType::SOLICITED_ONLY,
+            IStaInterface::UsdPublishType::SOLICITED_AND_UNSOLICITED)) {
+        wpa_printf(MSG_ERROR, "Unknown publish type received: %d",
+            static_cast<int>(publishConfig.publishType));
+        return false;
+    }
+    if (!isValidEnumValue(publishConfig.transmissionType,
+            IStaInterface::UsdPublishTransmissionType::UNICAST,
+            IStaInterface::UsdPublishTransmissionType::MULTICAST)) {
+        wpa_printf(MSG_ERROR, "Unknown transmission type received: %d",
+            static_cast<int>(publishConfig.transmissionType));
+        return false;
+    }
+    return true;
+}
+
+static bool validateUsdSubscribeConfig(IStaInterface::UsdSubscribeConfig subscribeConfig) {
+    if (!validateUsdBaseConfig(subscribeConfig.baseConfig)) {
+        return false;
+    }
+    if (!isValidEnumValue(subscribeConfig.subscribeType,
+            IStaInterface::UsdSubscribeType::PASSIVE_MODE,
+            IStaInterface::UsdSubscribeType::ACTIVE_MODE)) {
+        wpa_printf(MSG_ERROR, "Unknown subscribe type received: %d",
+            static_cast<int>(subscribeConfig.subscribeType));
+        return false;
+    }
+    return true;
+}
+
+static struct nan_publish_params convertAidlUsdPublishConfigToInternal(
+        IStaInterface::UsdPublishConfig publishConfig) {
+    struct nan_publish_params nanPublishParams;
+    nanPublishParams.unsolicited =
+        publishConfig.publishType == IStaInterface::UsdPublishType::SOLICITED_AND_UNSOLICITED
+            || publishConfig.publishType == IStaInterface::UsdPublishType::UNSOLICITED_ONLY;
+    nanPublishParams.solicited =
+        publishConfig.publishType == IStaInterface::UsdPublishType::SOLICITED_AND_UNSOLICITED
+            || publishConfig.publishType == IStaInterface::UsdPublishType::SOLICITED_ONLY;
+    nanPublishParams.solicited_multicast = nanPublishParams.solicited &&
+        publishConfig.transmissionType == IStaInterface::UsdPublishTransmissionType::MULTICAST;
+    nanPublishParams.ttl = publishConfig.baseConfig.ttlSec;
+    nanPublishParams.fsd = publishConfig.isFsd;
+    nanPublishParams.freq = publishConfig.baseConfig.defaultFreqMhz;
+    nanPublishParams.announcement_period = publishConfig.announcementPeriodMillis;
+    nanPublishParams.disable_events = !publishConfig.eventsEnabled;
+    // Pass the original pointer to the freq list, since the receiver will memcpy the data
+    nanPublishParams.freq_list = publishConfig.baseConfig.freqsMhz.empty()
+        ? NULL : publishConfig.baseConfig.freqsMhz.data();
+    return nanPublishParams;
+}
+
+static struct nan_subscribe_params convertAidlUsdSubscribeConfigToInternal(
+        IStaInterface::UsdSubscribeConfig subscribeConfig) {
+    struct nan_subscribe_params nanSubscribeParams;
+    nanSubscribeParams.active =
+        subscribeConfig.subscribeType == IStaInterface::UsdSubscribeType::ACTIVE_MODE;
+    nanSubscribeParams.ttl = subscribeConfig.baseConfig.ttlSec;
+    nanSubscribeParams.freq = subscribeConfig.baseConfig.defaultFreqMhz;
+    nanSubscribeParams.query_period = subscribeConfig.queryPeriodMillis;
+    // Pass the original pointer to the freq list, since the receiver will memcpy the data
+    nanSubscribeParams.freq_list = subscribeConfig.baseConfig.freqsMhz.empty()
+        ? NULL : subscribeConfig.baseConfig.freqsMhz.data();
+    return nanSubscribeParams;
+}
+
+static nan_service_protocol_type convertAidlServiceProtoTypeToInternal(
+        UsdServiceProtoType serviceProtoType) {
+    switch (serviceProtoType) {
+        case UsdServiceProtoType::GENERIC:
+            return NAN_SRV_PROTO_GENERIC;
+        case UsdServiceProtoType::CSA_MATTER:
+            return NAN_SRV_PROTO_CSA_MATTER;
+        default:
+            // Default case is not expected, due to the USD validation method
+            return NAN_SRV_PROTO_GENERIC;
+    };
+}
 
 #endif // MAINLINE_SUPPLICANT_USD_UTILS_H
diff --git a/wpa_supplicant/aidl/mainline/utils.h b/wpa_supplicant/aidl/mainline/utils.h
index 703b9ee..8b1fd19 100644
--- a/wpa_supplicant/aidl/mainline/utils.h
+++ b/wpa_supplicant/aidl/mainline/utils.h
@@ -11,15 +11,53 @@
 
 #include <aidl/android/system/wifi/mainline_supplicant/SupplicantStatusCode.h>
 
+extern "C"
+{
+#include "utils/common.h"
+#include "wpabuf.h"
+}
+
+namespace {
+// Custom deleter for wpabuf
+void freeWpaBuf(wpabuf *ptr) { wpabuf_free(ptr); }
+}
+
+using ::aidl::android::system::wifi::mainline_supplicant::SupplicantStatusCode;
+
+using wpabuf_unique_ptr = std::unique_ptr<wpabuf, void (*)(wpabuf *)>;
+
 inline ndk::ScopedAStatus createStatus(SupplicantStatusCode statusCode) {
-	return ndk::ScopedAStatus::fromServiceSpecificError(
-		static_cast<int32_t>(statusCode));
+    return ndk::ScopedAStatus::fromServiceSpecificError(
+        static_cast<int32_t>(statusCode));
 }
 
 inline ndk::ScopedAStatus createStatusWithMsg(
-	    SupplicantStatusCode statusCode, std::string msg) {
-	return ndk::ScopedAStatus::fromServiceSpecificErrorWithMessage(
-		static_cast<int32_t>(statusCode), msg.c_str());
+        SupplicantStatusCode statusCode, std::string msg) {
+    return ndk::ScopedAStatus::fromServiceSpecificErrorWithMessage(
+        static_cast<int32_t>(statusCode), msg.c_str());
+}
+
+// Check whether the container is within the maximum size
+template <typename T>
+inline bool checkContainerSize(const T& container, int maxSize) {
+    return container.size() <= maxSize;
+}
+
+// Check whether the enum value is within the specified range
+template <typename T>
+inline bool isValidEnumValue(T value, T enumRangeMin, T enumRangeMax) {
+    return static_cast<uint32_t>(value) >= static_cast<uint32_t>(enumRangeMin)
+        && static_cast<uint32_t>(value) <= static_cast<uint32_t>(enumRangeMax);
+}
+
+// Create a unique_ptr for a wpabuf ptr with a custom deleter
+inline wpabuf_unique_ptr createWpaBufUniquePtr(struct wpabuf *raw_ptr) {
+    return {raw_ptr, freeWpaBuf};
+}
+
+// Create a wpabuf ptr with a custom deleter, copying the data from the provided vector
+inline wpabuf_unique_ptr convertVectorToWpaBuf(const std::vector<uint8_t> &data) {
+    return createWpaBufUniquePtr(wpabuf_alloc_copy(data.data(), data.size()));
 }
 
 #endif // MAINLINE_SUPPLICANT_UTILS_H