/*
 *
 * Copyright (C) 2023-2025, Broadband Forum
 * Copyright (C) 2024-2025, Vantiva Technologies SAS
 * Copyright (C) 2025 Inango
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * 3. Neither the name of the copyright holder nor the names of its
 *    contributors may be used to endorse or promote products derived from
 *    this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
 * THE POSSIBILITY OF SUCH DAMAGE.
 *
 */

/**
 * \file uds.c
 *
 * Implements the Unix Domain Socket MTP (both server and client modes)
 *
 */

#ifdef ENABLE_UDS

#include <fcntl.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>

#include <errno.h>
#include <sys/socket.h>
#include <sys/un.h>
#include <sys/stat.h>
#include <string.h>

#include "common_defs.h"
#include "uds.h"
#include "device.h"
#include "dm_exec.h"
#include "vendor_defs.h"
#include "dllist.h"
#include "msg_handler.h"
#include "mtp_exec.h"
#include "os_utils.h"
#include "iso8601.h"
#include "usp_service.h"

#ifdef FD_PASSING_EXPERIMENTAL
#include "fd_vector.h"
#endif

//------------------------------------------------------------------------------
// R-UDS.5 - When a UNIX domain socket connection is closed or fails to be established, the USP Endpoint acting as a client MUST attempt to re-establish
// the UNIX domain socket within a random amount of time between 1 and 5 seconds
#define MIN_RETRY_INTERVAL 1
#define MAX_RETRY_INTERVAL 5
#define DONT_RETRY false
#define RETRY_LATER true
#define HANDSHAKE_TIMEOUT 30
#define TLV_HEADER_SIZE 5       // TLV header contains 1 byte type + 4 byte length

//------------------------------------------------------------------------------
// one of these for each instantiated UDS server
typedef struct
{
    int instance;          // Instance number in Device.UnixDomainSockets.UnixDomainSocket.{i}. INVALID denotes that this entry in the array isn't used
    int listen_sock;       // if this is an MTP (service) then listen on this socket for connections from controller
    uds_path_t path_type;  // Specifies whether this server is listening on the USP Broker's agent or controller path
    bool auth_required;    // Set to true if endpoints connecting to this server must provide password based authentication
    scheduled_action_t schedule_reconnect;  // Sets whether a UDS reconnect is scheduled after the send queue has cleared
    uds_conn_params_t next_conn_params;     // Connection parameters to use, the next time that a reconnect occurs
    char *socket_path;                      // Store the socket path to remove the socket file when closing the listening socket
} uds_server_t;

// Array of listening servers
static uds_server_t uds_servers[MAX_UDS_SERVERS];

//------------------------------------------------------------------------------
// Payload to send in UDS queue
typedef struct
{
    double_link_t link;        // Doubly linked list pointers. These must always be first in this structure
    uds_frame_t type;          // Identifies as either UDS error, handshake or message
    mtp_send_item_t item;      // Information about the content to send
    time_t expiry_time;        // Time at which this USP record should be removed from the queue
} uds_send_item_t;

//------------------------------------------------------------------------------
// Structure representing each client connection
typedef struct
{
    int instance;                                // Instance number in Device.UnixDomainSockets.UnixDomainSocket.{i}. INVALID denotes that this entry in the array isn't used
    uds_connection_type_t type;                  // Is this the server or client end of the connection
    uds_path_t path_type;                        // Specifies whether this connection is on the USP Broker's agent or controller path
    bool auth_required;                          // Set to true if this connection requires password based authentication
    int socket;                                  // socket connection to a server or client or INVALID if this UDS connection entry is not used
    int hdr_bytes_rxed;                          // number of bytes of sync header received on the socket
    int len_bytes_rxed;                          // number of bytes of length received on the socket
    unsigned char length_bytes[4];               // buffer to read length bytes into
    int payload_bytes_rxed;                      // number of bytes of payload received on the socket
    int payload_length;                          // calculated payload length from bitshifted bytes
    unsigned char *rx_buf;                       // transient buffer containing the payload of the UDS frame currently being received
    char *endpoint_id;                           // Endpoint ID at the other end of this connection
    double_linked_list_t usp_record_send_queue;  // Queue of USP Records to send to the endpoint
#ifdef FD_PASSING_EXPERIMENTAL
    struct msghdr *tx_buf;                       // Payload message containing USP message frame currently being send
    int* fd_buffer;                              // File descriptors buffer that received with payload
    int fd_count;                                // Amount of file descriptors
#else
    unsigned char *tx_buf;                       // transient buffer containing the USP message frame currently being sent
    int tx_bytes_sent;                           // Counts the number of bytes sent of the current USP Record to tx (i.e. at the head of the usp_record_send_queue)
#endif
    int tx_buf_len;                              // the length of the currently transmitting packet
    uds_frame_t tx_buf_type;                     // type of the uds frame being transmitted
    bool is_disconnect_record;                   // Is the transmitted usp message of disconnect record type
    unsigned conn_id;                            // Unique identifier for this connection. Used to ensure that USP responses are placed on the correct queue (or discarded if the connection has dropped)
    char *socket_path;                           // Store the socket path in case we need to reconnect, and also for logging
    time_t retry_timeout;                        // If we lose the connection and have to reconnect after a random timeout
    scheduled_action_t schedule_reconnect;       // Sets whether a UDS reconnect is scheduled after the send queue has cleared
    uds_conn_params_t next_conn_params;          // Connection parameters to use, the next time that a reconnect occurs
    time_t handshake_timeout;                    // Close the connection if we don't receive a handshake response after timeout
} uds_connection_t;

// Array of connections
static uds_connection_t uds_connections[MAX_UDS_SERVERS*MAX_USP_SERVICES];

//------------------------------------------------------------------------------
// Vector used to check the password that a USP Service provides
kv_vector_t uds_passwords;

//------------------------------------------------------------------------------
// Password to autheticate this endpoint on UDS MTP
char *uds_self_password = NULL;

//------------------------------------------------------------------------------
// Sync bytes identifying the start of a UDS frame
// If these are not present, then something is wrong and the connection should be dropped
static const char uds_frame_sync_bytes[4] = { 0x5F, 0x55, 0x53, 0x50 };

//------------------------------------------------------------------------------
// Mutex used to protect access to this component (MTP exec thread vs message handler)
// Should lock around UDS_xxx API functions to protect connections and item queues.  Private functions should not lock
static pthread_mutex_t uds_access_mutex;

//------------------------------------------------------------------------------
// Counter used to ensure that each uds connection has a unique identifier
static unsigned uds_conn_id_counter = 1;

//------------------------------------------------------------------------------
// Forward declarations. Note these are not static, because we need them in the symbol table for USP_LOG_Callstack() to show them
void ProcessUdsFrame(uds_connection_t *uc);
void ProcessUdsTLV(uds_connection_t *uc, uds_frame_t frame_type, unsigned char *record, unsigned record_length);
uds_connection_t *FindFreeUdsConnection(void);
uds_server_t *FindFreeUdsServer(void);
uds_server_t *FindUdsServerByInstanceId(int instance);
uds_connection_t *FindUdsClientByInstanceId(int instance);
void CloseUdsConnection(uds_connection_t *uc, bool retry);
int CreateUdsClient(uds_conn_params_t *ucp);
int StartUdsClient(uds_connection_t *uc);
int EnableUdsServer(uds_conn_params_t *ucp);
char *ValidateUdsEndpointID(char *endpointID, uds_path_t path_type);
void SendUdsErrorFrame(uds_connection_t *uc, char* errorString);
void SendUdsFrames(uds_connection_t *uc);
void ReadUdsFrames(uds_connection_t *uc);
int HandleUdsListeningSocketConnection(uds_server_t *us);
uds_connection_t *FindUdsConnectionByConnId(unsigned conn_id);
#ifdef FD_PASSING_EXPERIMENTAL
unsigned int PopUdsSendItem(uds_connection_t *uc);
#else
void PopUdsSendItem(uds_connection_t *uc);
#endif
unsigned CalcNextUdsConnectionId(void);
void RemoveUdsQueueItem(uds_connection_t *uc, uds_send_item_t *queued_msg);
void RemoveExpiredUdsMessages(uds_connection_t *uc);
bool IsUspRecordInUdsQueue(uds_connection_t *uc, unsigned char *pbuf, int pbuf_len);
void InitialiseUdsConnection(uds_connection_t *uc);
void InitialiseUdsServer(uds_server_t *us);
char *EndpointIdForLog(uds_connection_t *uc);
void QueueUspConnectRecord_Uds(uds_connection_t *uc);
void ProcessUdsHandshakeTLV(uds_connection_t *uc, unsigned char *record, unsigned record_length);
int ValidateUdsPayload(unsigned char *payload, int payload_len);
char *StrDupUdsTlvPayload(unsigned char *tlv_payload, int tlv_len);
void ProcessUdsTLV_Error(uds_connection_t *uc, unsigned char *tlv_payload, unsigned tlv_len);
void ProcessUdsTLV_UspRecord(uds_connection_t *uc, unsigned char *tlv_payload, unsigned tlv_len);
void ProcessUdsTLV_Echo(uds_connection_t *uc, unsigned char *tlv_payload, unsigned tlv_len);
void ProcessUdsTLV_HandshakePassword(uds_connection_t *uc, char *endpoint_id, char *password);
void PopulateUdsMtpConnection(mtp_conn_t *mtpc, uds_connection_t *uc);
void CopyUdsConnParams(uds_conn_params_t *dest, uds_conn_params_t *src);
void FreeUdsConnParams(uds_conn_params_t *ucp);
void FreeCurUdsTxItem(uds_connection_t *uc);

/*********************************************************************//**
**
** UDS_Init
**
** Called by DEVICE_UDS_Init data model during initialisation
**
** \param   None
**
** \return  USP_ERR_OK if successful
**
**************************************************************************/
int UDS_Init(void)
{
    uds_connection_t *uc;
    uds_server_t *us;
    int i;
    int err = USP_ERR_OK;

    KV_VECTOR_Init(&uds_passwords);

    // Mark all UDS connection slots as unused
    memset(uds_connections, 0, sizeof(uds_connections));

    for (i=0; i<NUM_ELEM(uds_connections); i++)
    {
        uc = &uds_connections[i];
        InitialiseUdsConnection(uc);
    }

    // Mark all UDS server slots as unused
    memset(uds_servers, 0, sizeof(uds_servers));
    for (i=0; i < NUM_ELEM(uds_servers); i++)
    {
        us = &uds_servers[i];
        InitialiseUdsServer(us);
    }

    // Exit if unable to create mutex protecting access to this subsystem
    err = OS_UTILS_InitMutex(&uds_access_mutex);
    if (err != USP_ERR_OK)
    {
        goto exit;
    }

exit:
    return err;
}

/*********************************************************************//**
**
** UDS_UpdateAllSockSet
**
** Updates the set of all UDS socket fds to read/write from
**
** \param   set - pointer to socket set structure to update with sockets to wait for activity on
**
** \return  USP_ERR_OK if successful
**
**************************************************************************/
void UDS_UpdateAllSockSet(socket_set_t *set)
{
    int i;
    uds_connection_t *uc;
    uds_server_t *us;
    bool responses_sent = false;
    uds_conn_params_t conn_params;

    OS_UTILS_LockMutex(&uds_access_mutex);

    // Exit if MTP thread has exited
    // NOTE: This check is not strictly ncessary, as only the MTP thread should be calling this function
    if (is_uds_mtp_thread_exited)
    {
        goto exit;
    }

    // Add all server listening sockets
    for (i=0; i<NUM_ELEM(uds_servers); i++)
    {
        us = &uds_servers[i];
        if (us->instance != INVALID)
        {
            if (us->schedule_reconnect == kScheduledAction_Activated)
            {
                // Its possible a listening server could be scheduled to restart when there are no valid connections
                // Default responses_sent to true to ensure the connection gets restarted in this case
                responses_sent = true;

                // we should only disable the listening server connection (and any connections associated with it) if there
                // are no pending messages being received or transmitted by any connections associated with the listening socket
                for (i=0; i<NUM_ELEM(uds_connections); i++)
                {
                    uc = &uds_connections[i];
                    if (uc->instance != INVALID)
                    {
                        // if the connection is a server connection belonging to the server socket instance
                        if ((uc->type == kUdsConnType_Server) && (uc->instance == us->instance))
                        {
                           // Determine if all responses have been sent on this connection, and update whether they have been sent on all connections
                           // Also check that we've not started to receive a UDS frame
                           responses_sent = ((uc->usp_record_send_queue.head == NULL) &&
                                             (uc->tx_buf == NULL) &&
                                             (uc->hdr_bytes_rxed==0));

                           if (responses_sent == false)
                           {
                              // there are pending messages in the message queue / rx buffer
                              // Break out and ignore this server instance until messages have been handled
                              USP_LOG_Info("Pending messages in the UDS server connection queue - cannot reconnect yet");
                              break;
                           }
                        }
                    }
                }

                if (responses_sent == true)
                {
                    // if all messages pertaining to this server socket have been sent then disable the connection
                    // and close all active connections associated with the listening server socket
                    USP_LOG_Info("UDS server connection parameters changed. Reconnecting instance %d", us->instance);

                    // make a deep copy of us->next_conn_params as these get freed in UDS_DisableConnection
                    CopyUdsConnParams(&conn_params, &us->next_conn_params);
                    UDS_DisableConnection(us->instance);

                    // start a new connection (which may be a client or a server)
                    UDS_EnableConnection(&conn_params);
                    FreeUdsConnParams(&conn_params);
                }
            }

            if (us->listen_sock != INVALID)
            {
                SOCKET_SET_AddSocketToReceiveFrom(us->listen_sock, MAX_SOCKET_TIMEOUT, set);
            }
        }
    }

    time_t next_wakeup_time = INVALID;

    // Check all connected client sockets
    for (i=0; i<NUM_ELEM(uds_connections); i++)
    {
        uc = &uds_connections[i];
        if (uc->instance != INVALID)
        {
            // Connecting clients (either connected or retrying) are checked for scheduled reconnect
            if (uc->type == kUdsConnType_Client)
            {
                // Determine if all responses have been sent on this connection, and update whether they have been sent on all connections
                // Also check that we've not started to receive a UDS frame
                responses_sent = ((uc->usp_record_send_queue.head == NULL) &&
                                  (uc->tx_buf == NULL) &&
                                  (uc->hdr_bytes_rxed==0));

                // If a reconnect is scheduled...
                if (uc->schedule_reconnect == kScheduledAction_Activated)
                {
                    // Perform a reconnect when all responses have been sent (and there are no incoming messages)
                    if (responses_sent)
                    {
                        USP_LOG_Info("UDS client connection parameters changed. Reconnecting instance %d", uc->instance);

                        // make a deep copy of uc->next_conn_params as these get freed in UDS_DisableConnection
                        CopyUdsConnParams(&conn_params, &uc->next_conn_params);
                        UDS_DisableConnection(uc->instance);

                        // start a new connection (which may be a client or a server)
                        UDS_EnableConnection(&conn_params);
                        FreeUdsConnParams(&conn_params);
                    }
                    else
                    {
                        USP_LOG_Info("Pending messages in the client connection queue - cannot reconnect yet");
                    }
                }
            }

            if (uc->socket != INVALID)
            {
                SOCKET_SET_AddSocketToReceiveFrom(uc->socket, MAX_SOCKET_TIMEOUT, set);

                // Only interested in writing if there are USP records in the queue/outgoing buffer
                if ((uc->usp_record_send_queue.head != NULL) || (uc->tx_buf != NULL))
                {
                    SOCKET_SET_AddSocketToSendTo(uc->socket, MAX_SOCKET_TIMEOUT, set);
                }
            }

            // If any connections are retrying or waiting for a handshake response then set a socket timeout
            if (uc->type == kUdsConnType_Client)
            {
                if (uc->retry_timeout != INVALID)
                {
                    if ((next_wakeup_time == INVALID) || (next_wakeup_time > uc->retry_timeout))
                    {
                       next_wakeup_time = uc->retry_timeout;
                    }
                }

                if (uc->handshake_timeout != INVALID)
                {
                    if ((next_wakeup_time == INVALID) || (next_wakeup_time > uc->handshake_timeout))
                    {
                       next_wakeup_time = uc->handshake_timeout;
                    }
                }
            }
        }
    }

    // we need to calculate when the socket next needs to wake and call processActivity
    // This is either on the next connection retry or when a handshake timeout occurs
    if (next_wakeup_time != INVALID)
    {
        time_t cur_time;
        time_t socket_timeout;
        cur_time = time(NULL);
        socket_timeout = next_wakeup_time - cur_time;

        // time_t is signed so it is possible for timeout to be negative if we overshot for some reason
        // Set a minimum timeout of 1 second to avoid spinning
        if (socket_timeout <= 1)
        {
            socket_timeout = 1;
        }

        SOCKET_SET_UpdateTimeout(socket_timeout*SECONDS, set);
    }

exit:
    OS_UTILS_UnlockMutex(&uds_access_mutex);
}

/*********************************************************************//**
**
** UDS_GetMTPForEndpointId
**
** Determines if the specified endpoint is connected to an active connection
** and if so returns the parameters specifying the MTP to use
**
** \param   endpoint_id - Endpoint ID of controller that maybe connected to agent's socket
** \param   mtpc - structure to update with MTP details, if the specified controller is connected to the agent's socket
**                or NULL if the caller is just trying to determine whether the controller is connected to the agent's socket
**                NOTE: the mtpc structure should be updated, not initialised.
**
** \return  USP_ERR_OK if specified endpoint is connected to this endpoint via UDS
**
**************************************************************************/
int UDS_GetMTPForEndpointId(char *endpoint_id, mtp_conn_t *mtpc)
{
    int i;
    int ret = USP_ERR_OK;
    uds_connection_t *uc;

    USP_ASSERT(mtpc != NULL);

    OS_UTILS_LockMutex(&uds_access_mutex);

    // Exit if MTP thread has exited
    if (is_uds_mtp_thread_exited)
    {
        ret = USP_ERR_INTERNAL_ERROR;
        goto exit;
    }

    for (i=0; i<NUM_ELEM(uds_connections); i++)
    {
        uc = &uds_connections[i];
        if (uc->instance != INVALID)
        {
            if ((uc->type != kUdsConnType_Invalid) && (uc->endpoint_id != NULL))
            {
                if (strcmp(uc->endpoint_id, endpoint_id)==0)
                {
                    PopulateUdsMtpConnection(mtpc, uc);
                    ret = USP_ERR_OK;
                    goto exit;
                }
            }
        }
    }

    ret = USP_ERR_INTERNAL_ERROR;

exit:
    OS_UTILS_UnlockMutex(&uds_access_mutex);
    return ret;
}

/*********************************************************************//**
**
** UDS_EnableConnection
**
** Starts a new Unix Domain Socket as either a server or client
**
** \param   ucp - UDS connection parameters
**
** \return  USP_ERR_OK if successful
**
**************************************************************************/
int UDS_EnableConnection(uds_conn_params_t *ucp)
{
    int err = USP_ERR_OK;

    OS_UTILS_LockMutex(&uds_access_mutex);

    // Exit if MTP thread has exited
    if (is_uds_mtp_thread_exited)
    {
        err = USP_ERR_INTERNAL_ERROR;
        goto exit;
    }

    switch(ucp->mode)
    {
        case kUdsConnType_Client:
            err = CreateUdsClient(ucp);
            break;

        case kUdsConnType_Server:
            err = EnableUdsServer(ucp);
            break;

        default:
        case kUdsConnType_Invalid:
            TERMINATE_BAD_CASE(ucp->mode);
            break;
    }

exit:
    OS_UTILS_UnlockMutex(&uds_access_mutex);
    return err;
}

/*********************************************************************//**
**
** UDS_DisableConnection
**
** Disconnects from the specified UDS connection and frees the specified connection
** This is called from DEVICE_UDS, if the connection has been disabled
**
** \param   instance - instance number in Device.UnixDomainSocket.UnixDomainSockets.{i}
**
** \return  USP_ERR_OK if successful
**
**************************************************************************/
int UDS_DisableConnection(int instance)
{
    int i;
    int err = USP_ERR_OK;
    uds_server_t *us;
    uds_connection_t *uc;

    OS_UTILS_LockMutex(&uds_access_mutex);

    // Exit if MTP thread has exited
    if (is_uds_mtp_thread_exited)
    {
        err = USP_ERR_INTERNAL_ERROR;
        goto exit;
    }

    // first check the server instances and close any matching listening sockets
    for (i=0; i<NUM_ELEM(uds_servers); i++)
    {
        us = &uds_servers[i];
        if ((us->instance != INVALID) && (us->instance == instance))
        {
            // and lastly close the server socket
            if (us->listen_sock != INVALID)
            {
                close(us->listen_sock);
                us->listen_sock = INVALID;
            }

            // delete any UDS file from the rootfs
            USP_ASSERT(us->socket_path);
            unlink(us->socket_path);
            USP_SAFE_FREE(us->socket_path);

            USP_SAFE_FREE(us->next_conn_params.path);

            InitialiseUdsServer(us);
            // there can only be one listening socket for a given instance so we can break out
            break;
        }
    }

    // The instance of a listening socket may be shared by any open connections on that socket
    // Iterate across all connections and close any that match the passed in instance that
    // either pertains to a listening socket connection or a client connection
    for (i=0; i<NUM_ELEM(uds_connections); i++)
    {
        uc = &uds_connections[i];
        if ((uc->instance != INVALID) && (uc->instance == instance))
        {
            CloseUdsConnection(uc, DONT_RETRY);
        }
    }

    err = USP_ERR_OK;

exit:
    OS_UTILS_UnlockMutex(&uds_access_mutex);
    return err;
}

/*********************************************************************//**
**
** UDS_ScheduleReconnect
**
** Signals that a UDS reconnect occurs when all queued messages have been sent
** See comment header above definition of scheduled_action_t for an explanation of how scheduled actions work, and why
**
** \param   ucp - pointer to data model parameters specifying the UDS connection
**
** \return  None
**
**************************************************************************/
void UDS_ScheduleReconnect(uds_conn_params_t *ucp)
{
    uds_connection_t *uc = NULL;
    uds_server_t *us = NULL;

    USP_LOG_Info("%s: scheduling reconnect on instance=%d path=%s, mode=%d", __FUNCTION__,ucp->instance, ucp->path, ucp->mode );

    OS_UTILS_LockMutex(&uds_access_mutex);

    // Exit if MTP thread has exited
    if (is_uds_mtp_thread_exited)
    {
        OS_UTILS_UnlockMutex(&uds_access_mutex);
        return;
    }

    // ucp->instance could pertain to either a connecting client or a server listening socket
    us = FindUdsServerByInstanceId(ucp->instance);
    if (us != NULL)
    {
        // instance pertains to a listening server.  We need to disconnect the server and create
        // a new connection instance for the next connection params (which may now be a client).
        us->next_conn_params.instance = ucp->instance;
        USP_SAFE_FREE(us->next_conn_params.path);
        us->next_conn_params.path = USP_STRDUP(ucp->path);
        us->next_conn_params.path_type = ucp->path_type;
        us->next_conn_params.mode = ucp->mode;
        us->schedule_reconnect = kScheduledAction_Signalled;
        mtp_reconnect_scheduled = true;     // Set flag to ensure that data model thread subsequently calls UDS_ActivateScheduledActions()
        goto exit;
    }

    // Exit if unable to find the specified UDS connection
    uc = FindUdsClientByInstanceId(ucp->instance);
    if (uc != NULL)
    {
        // instance pertains to a client.  We need to disconnect the client and create
        // a new connection instance for the next connection params (which may now be a server)
        uc->next_conn_params.instance = ucp->instance;
        USP_SAFE_FREE(uc->next_conn_params.path);
        uc->next_conn_params.path = USP_STRDUP(ucp->path);
        uc->next_conn_params.path_type = ucp->path_type;
        uc->next_conn_params.mode = ucp->mode;

        uc->schedule_reconnect = kScheduledAction_Signalled;
        mtp_reconnect_scheduled = true;     // Set flag to ensure that data model thread subsequently calls UDS_ActivateScheduledActions()
        goto exit;
    }

exit:
    OS_UTILS_UnlockMutex(&uds_access_mutex);

    // If successful, cause the MTP thread to wakeup from select().
    // We do this outside of the mutex lock to avoid an unnecessary task switch
    if ((uc != NULL) || (us != NULL))
    {
        MTP_EXEC_UdsWakeup();
    }
}

/*********************************************************************//**
**
** UDS_ProcessAllSocketActivity
**
** Processes the socket for the specified controller
**
** \param   set - pointer to socket set structure containing the sockets which need processing
**
** \return  Nothing
**
**************************************************************************/
void UDS_ProcessAllSocketActivity(socket_set_t *set)
{
    int i;
    uds_server_t *us;
    uds_connection_t *uc;

    OS_UTILS_LockMutex(&uds_access_mutex);

    // Exit if MTP thread has exited
    // NOTE: This check is not strictly ncessary, as only the MTP thread should be calling this function
    if (is_uds_mtp_thread_exited)
    {
        goto exit;
    }

    for (i=0; i<NUM_ELEM(uds_servers); i++)
    {
        us = &uds_servers[i];
        if ((us->instance != INVALID) && (us->listen_sock != INVALID))
        {
            // server is running - are there any connection attempts
            if (SOCKET_SET_IsReadyToRead(us->listen_sock, set))
            {
                HandleUdsListeningSocketConnection(us);
            }
        }
    }

    for (i=0; i<NUM_ELEM(uds_connections); i++)
    {
        uc = &uds_connections[i];
        if (uc->instance != INVALID)
        {
            if (uc->socket != INVALID)
            {
               // all active connections should attempt to read from the socket if data is available
               if (SOCKET_SET_IsReadyToRead(uc->socket, set))
               {
                   ReadUdsFrames(uc);
               }
            }

            // re-check uc->socket in case the connection was closed by ReadUdsFrames()
            if (uc->socket != INVALID)
            {
                if (SOCKET_SET_IsReadyToWrite(uc->socket, set))
                {
                    // Data in the queue/buffer and socket is ready to be written to
                    SendUdsFrames(uc);
                }
            }

            // re-check uc->socket in case the connection was closed by SendUdsFrames()
            if (uc->type == kUdsConnType_Client)
            {
                if (uc->retry_timeout != INVALID)
                {
                    // This is a valid client instance with an active retry timeout
                    if (uc->retry_timeout <= time(NULL))
                    {
                        uc->retry_timeout = INVALID;
                        USP_LOG_Info("%s: Retrying client socket connection on %s", __FUNCTION__, uc->socket_path);
                        StartUdsClient(uc);
                    }
                }

                if (uc->handshake_timeout != INVALID)
                {
                    // This is a valid client instance with an active retry timeout
                    if (uc->handshake_timeout <= time(NULL))
                    {
                        uc->handshake_timeout = INVALID;
                        USP_LOG_Info("%s: Retrying client socket handshake on %s", __FUNCTION__, uc->socket_path);
                        CloseUdsConnection(uc, RETRY_LATER);
                    }
                }
            }
        }
    }

exit:
    OS_UTILS_UnlockMutex(&uds_access_mutex);
}

/*********************************************************************//**
**
** UDS_QueueBinaryMessage
**
** Function called to queue a USP record on the specified UDS connection
**
** \param   msi - pointer to content to send
**                NOTE: Ownership of the payload buffer passes to this function, unless an error is returned
** \param   mtpc - pointer to structure containing details of where to send this message
** \param   expiry_time - time duration in which the message should expire
** \param   frame_type - type of the UDS frame being sent
**
** \return  USP_ERR_OK if successful
**
**************************************************************************/
int UDS_QueueBinaryMessage(mtp_send_item_t *msi, mtp_conn_t *mtpc, time_t expiry_time, uds_frame_t frame_type)
{
    uds_connection_t *uc;
    uds_send_item_t *send_item;
    int err = USP_ERR_GENERAL_FAILURE;
    bool is_duplicate = false;

    OS_UTILS_LockMutex(&uds_access_mutex);

    // Exit if MTP thread has exited
    if (is_uds_mtp_thread_exited)
    {
        err = USP_ERR_INTERNAL_ERROR;
        goto exit;
    }

    // Exit if unable to determine which UDS connection to put the record on.
    // This could happen if the UDS connection dropped or was deleted in the data model whilst processing the USP Request (that this message is the associated USP response)
    uc = FindUdsConnectionByConnId(mtpc->uds.conn_id);
    if (uc == NULL)
    {
        err = USP_ERR_INTERNAL_ERROR;
        goto exit;
    }
    USP_ASSERT(uc->instance == mtpc->uds.instance);

    // Remove any queued messages that have expired
    RemoveExpiredUdsMessages(uc);

    // Do not add this message to the queue, if it is already present in the queue
    // This situation could occur if a notify is being retried to be sent, but is already held up in the queue pending sending
    if(frame_type == kUdsFrameType_UspRecord)
    {
       is_duplicate = IsUspRecordInUdsQueue(uc, msi->pbuf, msi->pbuf_len);
       if (is_duplicate)
       {
           USP_FREE(msi->pbuf);
           err = USP_ERR_OK;
           goto exit;
       }
    }

    // Add USP Record to queue
    send_item = USP_MALLOC(sizeof(uds_send_item_t));
    memset(send_item, 0, sizeof(uds_send_item_t));
    send_item->item = *msi;  // NOTE: Ownership of the payload buffer passes to the UDS message queue
    send_item->expiry_time = expiry_time;
    send_item->type = frame_type;
    if(send_item->type == kUdsFrameType_Error || msi->content_type == kMtpContentType_ConnectRecord)
    {
       DLLIST_LinkToHead(&uc->usp_record_send_queue, send_item);
    }
    else
    {
       DLLIST_LinkToTail(&uc->usp_record_send_queue, send_item);
    }
    err = USP_ERR_OK;

exit:
    OS_UTILS_UnlockMutex(&uds_access_mutex);

    // kick the MTP thread to check the write status of the sending socket
    if (err == USP_ERR_OK)
    {
        MTP_EXEC_UdsWakeup();
    }

    return err;
}
/*********************************************************************//**
**
** UDS_ActivateScheduledActions
**
** Called when all USP response messages have been queued.
** This function activates all scheduled actions which have been signalled
** See comment header above definition of scheduled_action_t for an explanation of how scheduled actions work, and why
**
** \param   None
**
** \return  None
**
**************************************************************************/
void UDS_ActivateScheduledActions(void)
{
    int i;
    uds_connection_t *uc;
    uds_server_t *us;
    bool wakeup = false;

    OS_UTILS_LockMutex(&uds_access_mutex);

    // Exit if MTP thread has exited
    if (is_uds_mtp_thread_exited)
    {
        OS_UTILS_UnlockMutex(&uds_access_mutex);
        return;
    }

    // Iterate over all UDS connections, activating all reconnects and resubscribes which have been signalled
    for (i=0; i<NUM_ELEM(uds_connections); i++)
    {
        uc = &uds_connections[i];
        if (uc->instance != INVALID)
        {
           if (uc->schedule_reconnect == kScheduledAction_Signalled)
           {
               uc->schedule_reconnect = kScheduledAction_Activated;
               wakeup = true;
           }
        }
    }

    for (i=0; i<NUM_ELEM(uds_servers); i++)
    {
        us = &uds_servers[i];
        if (us->instance != INVALID)
        {
            if (us->schedule_reconnect == kScheduledAction_Signalled)
            {
                us->schedule_reconnect = kScheduledAction_Activated;
                wakeup = true;
            }
        }
    }

    OS_UTILS_UnlockMutex(&uds_access_mutex);

    // Wakeup the UDS MTP thread, so that it can process the reconnect
    // (This is done outside of the mutex protection, as a slight optimization to avoid unnecessary task switches)
    if (wakeup)
    {
        MTP_EXEC_UdsWakeup();
    }
}

/*********************************************************************//**
**
** UDS_AreAllResponsesSent
**
** Determines whether all responses have been sent, and that there are no outstanding incoming messages
**
** \param   None
**
** \return  true if all responses have been sent
**
**************************************************************************/
bool UDS_AreAllResponsesSent(void)
{
    int i;
    uds_connection_t *uc;
    bool responses_sent;
    bool all_responses_sent = true;  // Assume that all responses have been sent on all connections

    OS_UTILS_LockMutex(&uds_access_mutex);

    // Exit if MTP thread has exited
    if (is_uds_mtp_thread_exited)
    {
        all_responses_sent = true;
        goto exit;
    }

    // Add all connected client sockets
    for (i=0; i< NUM_ELEM(uds_connections); i++)
    {
        uc = &uds_connections[i];
        // include ALL connections.  Connected or retrying clients, and also connections pertaining to server sockets
        // Note we may want to modify this to ignore retrying clients as potentially this could deadlock if a connection is stuck retrying
        if (uc->instance != INVALID)
        {
            // Determine if all responses have been sent on this connection, and update whether they have been sent on all connections
            // Also check that we've not started to receive a UDS frame
            responses_sent = ((uc->usp_record_send_queue.head == NULL) &&
                              (uc->tx_buf == NULL) &&
                              (uc->hdr_bytes_rxed==0));

            if (responses_sent == false)
            {
                all_responses_sent = false;
                break;
            }
        }
    }

exit:
    OS_UTILS_UnlockMutex(&uds_access_mutex);

    return all_responses_sent;
}

/*********************************************************************//**
**
** UDS_Destroy
**
** Frees all memory associated with this component and closes all sockets
**
** \param   None
**
** \return  None
**
**************************************************************************/
void UDS_Destroy(void)
{
    int i;
    uds_connection_t *uc;
    uds_server_t *us;

    OS_UTILS_LockMutex(&uds_access_mutex);  // Ensure that the data model is held off accessing this module's data structures until after we have destroyed them

    // disable any "Connect"ing connections and client connections from listening servers
    for (i=0; i< NUM_ELEM(uds_connections); i++)
    {
        uc = &uds_connections[i];
        if (uc->instance != INVALID)
        {
            UDS_DisableConnection(uc->instance);
        }
    }

    // disable any "Listen"ing server connections
    for (i=0; i < NUM_ELEM(uds_servers); i++)
    {
        us = &uds_servers[i];
        if (us->instance != INVALID)
        {
            UDS_DisableConnection(us->instance);
        }
    }

    // Free all expected passwords and our own UDS MTP password
    KV_VECTOR_Destroy(&uds_passwords);
    USP_SAFE_FREE(uds_self_password);

    // Prevent the data model from making any other changes to the MTP thread
    is_uds_mtp_thread_exited = true;

    OS_UTILS_UnlockMutex(&uds_access_mutex);
}

/*********************************************************************//**
**
** UDS_PathTypeToString
**
** Returns a textual description of the specified type of path, for use by debug
** NOTE: This function may be called from any thread
**
** \param   path_type - type of path
**
** \return  None
**
**************************************************************************/
char *UDS_PathTypeToString(uds_path_t path_type)
{
    char *str;

    switch(path_type)
    {
        case kUdsPathType_BrokersAgent:
            str = "Broker's Agent path";
            break;

        case kUdsPathType_BrokersController:
            str = "Broker's Controller path";
            break;

        default:
        case kUdsPathType_Invalid:
            str = "Unknown";
            break;
    }

    return str;
}

/*********************************************************************//**
**
** UDS_GetMtpStatus
**
** Function called to get the value of Device.LocalAgent.MTP.{i}.Status for a UDS connection
**
** \param   instance - instance number of the connection in Device.UnixDomainSockets.UnixDomainSocket.{i}
**
** \return  Status of the UDS connection
**
**************************************************************************/
mtp_status_t UDS_GetMtpStatus(int instance)
{
    int i;
    mtp_status_t status;
    uds_connection_t *uc;
    uds_server_t *us;

    OS_UTILS_LockMutex(&uds_access_mutex);

    // Exit, returning the status, if this instance is a server
    for (i=0; i<NUM_ELEM(uds_servers); i++)
    {
        us = &uds_servers[i];
        if ((us->instance != INVALID) && (us->instance == instance))
        {
            // NOTE: At startup, the USP Broker exits if any UDS servers fail to start. So it's not currently possible to
            // return kMtpStatus_Down. The code below will cope if this changes in the future.
            status = (us->listen_sock == INVALID) ? kMtpStatus_Down : kMtpStatus_Up;
            goto exit;
        }
    }

    // Exit, returning the status, if this instance is a client
    for (i=0; i<NUM_ELEM(uds_connections); i++)
    {
        uc = &uds_connections[i];
        if ((uc->instance != INVALID) && (uc->instance == instance) && (uc->type==kUdsConnType_Client))
        {
            status = (uc->socket == INVALID) ? kMtpStatus_Down : kMtpStatus_Up;
            goto exit;
        }
    }

    // If the code gets here, then no matching entry was found
    status = kMtpStatus_Error;

exit:
    OS_UTILS_UnlockMutex(&uds_access_mutex);
    return status;
}

/*********************************************************************//**
**
** UDS_AddAuthPassword
**
** Adds the password for a USP Service connecting to this endpoint,
** so that we can check it when it connects
**
** \param   endpoint_id - Identifies the USP Service
** \param   password - Password for the USP Service
**
** \return  None
**
**************************************************************************/
void UDS_AddAuthPassword(char *endpoint_id, char *password)
{
    OS_UTILS_LockMutex(&uds_access_mutex);

    KV_VECTOR_Add(&uds_passwords, endpoint_id, password);

    OS_UTILS_UnlockMutex(&uds_access_mutex);
}

/*********************************************************************//**
**
** UDS_RemoveAuthPassword
**
** Removes the password for a USP Service connecting to this endpoint.
** If the USP Service connects on an 'AuthRequired' UDS socket, then it will fail to connect
**
** \param   endpoint_id - Identifies the USP Service
**
** \return  None
**
**************************************************************************/
void UDS_RemoveAuthPassword(char *endpoint_id)
{
    OS_UTILS_LockMutex(&uds_access_mutex);

    KV_VECTOR_Remove(&uds_passwords, endpoint_id);

    OS_UTILS_UnlockMutex(&uds_access_mutex);
}

/*********************************************************************//**
**
** UDS_ModifAuthPassword
**
** Modifies the expected password for a USP Service connecting to this endpoint
**
** \param   endpoint_id - Identifies the USP Service
** \param   password - Password for the USP Service
**
** \return  None
**
**************************************************************************/
void UDS_ModifyAuthPassword(char *endpoint_id, char *password)
{
    OS_UTILS_LockMutex(&uds_access_mutex);

    KV_VECTOR_Replace(&uds_passwords, endpoint_id, password);

    OS_UTILS_UnlockMutex(&uds_access_mutex);
}

/*********************************************************************//**
**
** UDS_SetSelfPassword
**
** Mofifies the password used in this endpoint's UDS handshake when connecting to another USP Endpoint
**
** \param   password - Password for this endpoint
**
** \return  None
**
**************************************************************************/
void UDS_SetSelfPassword(char *password)
{
    OS_UTILS_LockMutex(&uds_access_mutex);

    USP_ASSERT(password != NULL);
    USP_SAFE_FREE(uds_self_password);
    uds_self_password = USP_STRDUP(password);

    OS_UTILS_UnlockMutex(&uds_access_mutex);
}


/*********************************************************************//**
**
** EnableUdsServer
**
** Private internal function to start a "Listening" mode domain socket
**
** \param   ucp - pointer to data model parameters specifying the connection to disable
**
** \return  None
**
**************************************************************************/
int EnableUdsServer(uds_conn_params_t *ucp)
{
    int err = USP_ERR_INTERNAL_ERROR;
    int result;
    struct sockaddr_un addr;
    uds_server_t *us;
    mode_t current_mask;

    // Exit if unable to find an unused server slot
    us = FindFreeUdsServer();
    if (us == NULL)
    {
        USP_LOG_Error("%s: No free server elements available", __FUNCTION__);
        return USP_ERR_INTERNAL_ERROR;
    }

    // Initialise the UDS server structure
    InitialiseUdsServer(us);
    us->instance = ucp->instance;
    us->path_type = ucp->path_type;
    USP_ASSERT(us->socket_path == NULL);
    us->socket_path = USP_STRDUP(ucp->path);
    us->auth_required = ucp->auth_required;
    us->next_conn_params.instance = ucp->instance;
    us->next_conn_params.path = USP_STRDUP(ucp->path);
    us->next_conn_params.path_type = ucp->path_type;
    us->next_conn_params.mode = ucp->mode;
    us->next_conn_params.auth_required = ucp->auth_required;
    us->next_conn_params.registration_restricted = ucp->registration_restricted; // NOTE: not actually used by UDS MTP, but copied for completeness

    // Create a new server socket with domain: AF_UNIX, type: SOCK_STREAM, protocol: 0
    us->listen_sock = socket(AF_UNIX, SOCK_STREAM, 0);
    if (us->listen_sock == INVALID)
    {
        USP_ERR_ERRNO("socket", errno);
        return USP_ERR_INTERNAL_ERROR;
    }

    // Exit if unable to ensure that all directories used by the UDS path have been created
    err = OS_UTILS_CreateDirFromFilename(ucp->path);
    if (err != USP_ERR_OK)
    {
        goto exit;
    }

    // Note normally we would expect any socket files to be cleaned up when the socket was gracefully closed
    // however - it is possible the file exists due to ungraceful exit of the previous execution
    unlink(ucp->path);

    // Fill in the unix domain socket path
    memset(&addr, 0, sizeof(struct sockaddr_un));
    addr.sun_family = AF_UNIX;
    USP_STRNCPY(addr.sun_path, ucp->path, sizeof(addr.sun_path) - 1);

    // Exit if unable to bind the socket to the required path
    // NOTE: Temporarily change file creation permissions so that this process (running as root) creates a socket that can be accessed by non-root users
    current_mask = umask(0);
    result = bind(us->listen_sock, (struct sockaddr *) &addr, sizeof(struct sockaddr_un));
    if (result == -1)
    {
        USP_ERR_ERRNO("bind", errno);
        err = USP_ERR_INTERNAL_ERROR;
        goto exit;
    }
    umask(current_mask);

    // this is a passive socket that creates active sockets connections
    #define MAX_PENDING_CONNECTIONS     5
    result = listen(us->listen_sock, MAX_PENDING_CONNECTIONS);
    if (result == -1)
    {
        USP_ERR_ERRNO("listen", errno);
        err = USP_ERR_INTERNAL_ERROR;
        goto exit;
    }

    // we now have a passive socket listening for connections from services at SERVER_SOCKET_PATH
    err = USP_ERR_OK;

exit:
    // clean up any resources allocated
    if (err != USP_ERR_OK)
    {
        USP_ASSERT(us->listen_sock != INVALID);
        close(us->listen_sock);
        us->listen_sock = INVALID;      // Mark the slot status as 'Error'
    }

    return err;
}

/*********************************************************************//**
**
** CreateUdsClient
**
** Private internal function to create a "Connect" client connection instance
**
** \param   ucp - pointer to data model parameters specifying the domain socket path
**
** \return  None
**
**************************************************************************/
int CreateUdsClient(uds_conn_params_t *ucp)
{
    int err = USP_ERR_OK;
    uds_connection_t *uc = NULL;

    // Exit if unable to find an unused connection slot
    uc = FindFreeUdsConnection();
    if (uc == NULL)
    {
        USP_LOG_Error("%s: Too many connections - ignoring connect request", __FUNCTION__);
        err = USP_ERR_INTERNAL_ERROR;
        goto exit;
    }

    memset(uc, 0, sizeof(uds_connection_t));

    InitialiseUdsConnection(uc);

    uc->instance = ucp->instance;
    uc->conn_id = CalcNextUdsConnectionId();
    uc->path_type = ucp->path_type;
    uc->auth_required = ucp->auth_required;
    uc->socket_path = USP_STRDUP(ucp->path);

    err = StartUdsClient(uc);

exit:
    return err;
}

/*********************************************************************//**
**
** StartUdsClient
**
** Private internal function to start a "Connect" mode domain socket
** Called when creating a new client connection, or retrying an existing connection
**
** \param   ucp - pointer to data model parameters specifying the domain socket path
**
** \return  USP_ERR_OK if the connection is established or a retry triggered
**
**************************************************************************/
int StartUdsClient(uds_connection_t *uc)
{
    int err = USP_ERR_INTERNAL_ERROR;
    int result;
    struct sockaddr_un addr;
    uds_send_item_t *send_item = NULL;

    // type might be kUdsConnType_Retry if retrying after being disconnected
    uc->type = kUdsConnType_Client;

    // Create a new client connecting socket with domain: AF_UNIX, type: SOCK_STREAM, protocol: 0
    uc->socket = socket(AF_UNIX, SOCK_STREAM | SOCK_NONBLOCK, 0);
    if (uc->socket == -1)
    {
        USP_ERR_ERRNO("socket", errno);
        err = USP_ERR_INTERNAL_ERROR;
        goto exit;
    }

    // Fill in the unix domain socket path
    USP_ASSERT(uc->socket_path != NULL);
    memset(&addr, 0, sizeof(struct sockaddr_un));
    addr.sun_family = AF_UNIX;
    USP_STRNCPY(addr.sun_path, uc->socket_path, sizeof(addr.sun_path));

    // Exit if unable to connect
    result = connect(uc->socket, (struct sockaddr *) &addr, sizeof(struct sockaddr_un));
    if (result == -1)
    {
        // This may mean that the service we're connecting to isn't available yet
        USP_LOG_Warning("%s: cannot connect to socket path %s", __FUNCTION__, uc->socket_path);
        // close connection, set retry = true to re-attempt connection automatically
        CloseUdsConnection(uc, RETRY_LATER);
        // Because we are retrying the connection its okay to return success here
        err = USP_ERR_OK;
        goto exit;
    }

    // Add USP Record to end of queue
    send_item = USP_MALLOC(sizeof(uds_send_item_t));
    memset(send_item, 0, sizeof(uds_send_item_t));
    send_item->expiry_time = END_OF_TIME;
    send_item->type = kUdsFrameType_Handshake;
    DLLIST_LinkToHead(&uc->usp_record_send_queue, send_item);

    // UDS-18 if we don't receive a handshake within 30 seconds then we need to close the connection
    time_t cur_time;
    cur_time = time(NULL);
    uc->handshake_timeout = cur_time + HANDSHAKE_TIMEOUT;
    // kick the MTP thread here to trigger recalc of the handshake timeout in UDS_UpdateAllSockSet
    MTP_EXEC_UdsWakeup();

    err = USP_ERR_OK;

exit:
    // If a non-recoverable error occurred connecting to the server then clean up and return an error
    if (err != USP_ERR_OK)
    {
        CloseUdsConnection(uc, DONT_RETRY);
    }

    return err;
}

/*********************************************************************//**
**
** FindFreeUdsConnection
**
** Private internal function to find a free UDS connection element in the array
**
** \param   None
**
** \return  a pointer to the connection, or NULL if none are found
**
**************************************************************************/
uds_connection_t *FindFreeUdsConnection(void)
{
    int i;
    uds_connection_t *uc;

    for (i=0; i<NUM_ELEM(uds_connections); i++)
    {
        uc = &uds_connections[i];
        if (uc->instance == INVALID)
        {
            return uc;
        }
    }

    // If the code gets here, then no free entry was found
    return NULL;
}

/*********************************************************************//**
**
** FindUdsConnectionByConnId
**
** Finds the UDS connection matching the specified connection_id
**
** \param   conn_id - unique identifier for each connection
**
** \return  a pointer to the connection, or NULL if none are found
**
**************************************************************************/
uds_connection_t *FindUdsConnectionByConnId(unsigned conn_id)
{
    int i;
    uds_connection_t *uc;

    for (i=0; i<NUM_ELEM(uds_connections); i++)
    {
        uc = &uds_connections[i];
        if ((uc->instance != INVALID) && (uc->conn_id == conn_id))
        {
            return uc;
        }
    }

    // If the code gets here, then no free entry was found
    return NULL;
}

/*********************************************************************//**
**
** FindFreeUdsServer
**
** Private internal function to find a free UDS server element in the array
**
** \param   None
**
** \return  a pointer to the server, or NULL if none are found
**
**************************************************************************/
uds_server_t *FindFreeUdsServer(void)
{
    int i;
    uds_server_t *us;

    for (i=0; i<NUM_ELEM(uds_servers); i++)
    {
        us = &uds_servers[i];
        if (us->instance == INVALID)
        {
            return us;
        }
    }

    // If the code gets here, then no free entry was found
    return NULL;
}

/*********************************************************************//**
**
** QueueUspConnectRecord_Uds
**
** Adds the USP connect record at the front of the queue, ensuring that there is only one connect record in the queue
**
** \param   uc - Pointer to uds connection to send the connect record to
**
** \return  None
**
**************************************************************************/
void QueueUspConnectRecord_Uds(uds_connection_t *uc)
{
    uds_send_item_t *cur_msg;
    uds_send_item_t *next_msg;
    mtp_content_type_t type;
    uds_send_item_t *send_item;

    // Iterate over USP Records in the queue, removing all stale connect and disconnect records
    // A connect or disconnect record may still be in the queue if the connection failed before the record was fully sent
    cur_msg = (uds_send_item_t *) uc->usp_record_send_queue.head;
    while (cur_msg != NULL)
    {
        // Save pointer to next message, as we may remove the current message
        next_msg = (uds_send_item_t *) cur_msg->link.next;

        // Remove current message if it is a connect or disconnect record
        type = cur_msg->item.content_type;
        if (IsUspConnectOrDisconnectRecord(type))
        {
            RemoveUdsQueueItem(uc, cur_msg);
        }

        // Move to next message in the queue
        cur_msg = next_msg;
    }

    // Create the UDS USP Connect record
    send_item = USP_MALLOC(sizeof(uds_send_item_t));
    memset(send_item, 0, sizeof(uds_send_item_t));
    send_item->expiry_time = END_OF_TIME;
    send_item->type = kUdsFrameType_UspRecord;
    USPREC_UdsConnect_Create(uc->endpoint_id, &send_item->item);

    // Add the new connect record to the queue
    DLLIST_LinkToHead(&uc->usp_record_send_queue, send_item);
}

/*********************************************************************//**
**
** CloseUdsConnection
**
** Private internal function to close a connection
**
** \param   uc - Pointer to the connection to close
** \param   retry - whether to attempt to retry connecting (only relevant for client connections, ignored for server connections)
**
** \return  None
**
**************************************************************************/
void CloseUdsConnection(uds_connection_t *uc, bool retry)
{
    // Close the socket
    if (uc->socket != INVALID)
    {
        close(uc->socket);
        uc->socket = INVALID;
    }

#ifdef FD_PASSING_EXPERIMENTAL
    if (uc->fd_buffer)
    {
        FD_VECTOR_Close(uc->fd_buffer, uc->fd_count);
        uc->fd_buffer = NULL;
        uc->fd_count = 0;
    }
#endif

    // Inform the rest of the system that an endpoint has disconnected, unless this was a graceful shutdown
    // We don't post the message for graceful shutdown, because we don't want any active requests to be removed from
    // the request table, as the operation complete (indicating failure) will not be sent because UDS has shutdown
    // Instead the operation complete is sent the next time that we start up
    // Also don't post if the connection was closed before successful UDS handshake (and hence remote endpoint_id is unknown)
    if ((mtp_exit_scheduled != kScheduledAction_Activated) && (uc->endpoint_id != NULL))
    {
        DM_EXEC_PostUdsDisconnected(uc->endpoint_id, uc->path_type);
    }

    // flush any rx/tx buffers
    USP_SAFE_FREE(uc->rx_buf);
    FreeCurUdsTxItem(uc);
    USP_SAFE_FREE(uc->endpoint_id);

    // Flush all queued outgoing USP messages
    // Unlike other MTPs, For UDS we purge the list regardless of retrying or not.  Unlike other MTPS, for
    // UDS it is highly likely that loss of connection is due to either the client or server crashing and
    // potentially restarting.  On loss of connection any registered UDS services should be removed from
    // the datamodel and reregistered on successful reconnection.
    while (uc->usp_record_send_queue.head != NULL)
    {
       RemoveUdsQueueItem(uc, (uds_send_item_t *) uc->usp_record_send_queue.head);
    }

    // only attempt to retry/reconnect if we are the client side of the connection
    // Server side connection should be invalidated and a new connection established on a retry from a client
    if ((uc->type == kUdsConnType_Client) && (retry == true))
    {
        unsigned int delay;
        // retry state is special and indicates a valid client connection instance that is currently disconnected
        // No packets will be sent or received in this state or until the state returns to kUdsConnType_Client
        // which can only happen after a successful reconnection to a server.
        // delay must be between 1 and 5 seconds
        delay = (rand() % (MAX_RETRY_INTERVAL-MIN_RETRY_INTERVAL)) + MIN_RETRY_INTERVAL;
        uc->retry_timeout =  time(NULL) + delay;
        USP_LOG_Info("%s: Retrying connection in %d seconds for %s", __FUNCTION__, delay, uc->socket_path);
        // kick the MTP thread here to trigger recalc of the socket timeout in UDS_UpdateAllSockSet
        MTP_EXEC_UdsWakeup();
    }
    else
    {
        // This is a server type connection, or a client connection that we don't want to reconnect
        USP_SAFE_FREE(uc->socket_path);
        USP_SAFE_FREE(uc->next_conn_params.path);
        InitialiseUdsConnection(uc);
    }

    return;
}

/*********************************************************************//**
**
** ReadUdsFrames
**
** Private internal function read data from any UDS connections with data available
**
** \param   uc - UDS connection which received some data
**
** \return  None
**
**************************************************************************/
void ReadUdsFrames(uds_connection_t *uc)
{
    int i;
    int num_bytes = 0;

#ifdef FD_PASSING_EXPERIMENTAL
    struct msghdr bmsg = {0};
    struct cmsghdr* cmsg;
    struct iovec iov;
    union {
        struct cmsghdr align;
        char buffer[CMSG_SPACE(sizeof(int) * MAX_UDS_FDS)];
    } control_un;
    bmsg.msg_iov = &iov;
    bmsg.msg_iovlen = 1;
    bmsg.msg_control = control_un.buffer;
    bmsg.msg_controllen = CMSG_SPACE(sizeof(int) * MAX_UDS_FDS);
    int fd_amount = 0;
#endif

    USP_ASSERT(uc->socket != INVALID);

    // the following implementation breaks the message down into header, length and payload to make
    // it easier to follow.  It could potentially be optimised to read a larger block of data into
    // a temporary buffer and then process that buffer in one iteration - though we cannot know the
    // length of the payload up front, so there's always the possibility of having to iterate several
    // times in order to pull in the entire frame.  This implementation takes a minimum of 3 iterations.


    // All UDS messages contain an 8 byte Header including a sync word followed by remaining bytes
    if (uc->hdr_bytes_rxed < 4)
    {
        int bytes_outstanding;
        char buf[4];

        bytes_outstanding = sizeof(uds_frame_sync_bytes) - uc->hdr_bytes_rxed;
#ifdef FD_PASSING_EXPERIMENTAL
        iov.iov_base = buf;
        iov.iov_len = bytes_outstanding;
        num_bytes = recvmsg(uc->socket, &bmsg, 0);
        if (num_bytes <= 0) // -1 = error, 0 = other end closed
        {
            USP_LOG_Warning("%s: Endpoint (%s) disconnected from %s", __FUNCTION__, EndpointIdForLog(uc), uc->socket_path);
            CloseUdsConnection(uc, RETRY_LATER);
            return;
        }

        if (uc->fd_buffer == NULL)
        {
            uc->fd_buffer = USP_MALLOC(sizeof(int) * MAX_UDS_FDS);
            if (uc->fd_buffer == NULL)
            {
                USP_LOG_Warning("%s: Failed to allocate buffer for file descriptors", __FUNCTION__);
                SendUdsErrorFrame(uc, "Failed to allocate buffer for file descriptors");
                CloseUdsConnection(uc, RETRY_LATER);
                return;
            }

            for (cmsg = CMSG_FIRSTHDR(&bmsg); cmsg != NULL; cmsg = CMSG_NXTHDR(&bmsg, cmsg))
            {
                if ((cmsg->cmsg_level == SOL_SOCKET) && (cmsg->cmsg_type == SCM_RIGHTS))
                {
                    if (cmsg->cmsg_len >= CMSG_LEN(sizeof(int)))
                    {
                        fd_amount = (cmsg->cmsg_len - CMSG_LEN(0)) / sizeof(int);
                        int* fd = (int *)CMSG_DATA(cmsg);
                        for (uc->fd_count = 0; uc->fd_count < fd_amount; uc->fd_count++)
                        {
                            uc->fd_buffer[uc->fd_count] = *fd;
                            fd++;
                        }
                    }
                    else
                    {
                        break;
                    }
                }
            }

            if ((bmsg.msg_flags & MSG_CTRUNC) != 0)
            {
                FD_VECTOR_Close(uc->fd_buffer, uc->fd_count);
                uc->fd_buffer = NULL;
                uc->fd_count = -1;
            }
        }
#else
        num_bytes = recv(uc->socket, buf, bytes_outstanding, 0);
        if (num_bytes <= 0) // -1 = error, 0 = other end closed
        {
            USP_LOG_Warning("%s: Endpoint (%s) disconnected from %s", __FUNCTION__, EndpointIdForLog(uc), uc->socket_path);
            CloseUdsConnection(uc, RETRY_LATER);
            return;
        }
#endif

        // Count the number of correct sync bytes received
        // If any of the sync bytes are incorrect, then disconnect
        for (i=0; i < num_bytes && (uc->hdr_bytes_rxed < 4) ; i++ )
        {
            if (buf[i] == uds_frame_sync_bytes[uc->hdr_bytes_rxed])
            {
                uc->hdr_bytes_rxed++;
            }
            else
            {
                USP_LOG_Error("%s: UDS Sync bytes incorrect. Disconnecting %s from %s", __FUNCTION__, EndpointIdForLog(uc), uc->socket_path);
                CloseUdsConnection(uc, RETRY_LATER);
                return;
            }
        }
    }

    if ((uc->hdr_bytes_rxed == 4) && (uc->len_bytes_rxed < 4))
    {
        unsigned char *buf;
        int len;

        // great - we've got our 4 byte header so now we need to read the length 4 bytes
        buf = &uc->length_bytes[uc->len_bytes_rxed];
        len = 4 - uc->len_bytes_rxed;
        num_bytes = recv(uc->socket, buf, len, 0);
        if (num_bytes <= 0) // -1 = error, 0 = other end closed
        {
            USP_LOG_Warning("%s: Endpoint (%s) disconnected from %s", __FUNCTION__, EndpointIdForLog(uc), uc->socket_path);
            CloseUdsConnection(uc, RETRY_LATER);
            return;
        }

        uc->len_bytes_rxed += num_bytes;

        if (uc->len_bytes_rxed == 4)
        {
            // convert from the byte stream using correct endian
            uc->payload_length = CONVERT_4_BYTES(uc->length_bytes);
            if(uc->payload_length < TLV_HEADER_SIZE)
            {
                // The minimum valid frame payload length is at least one record which contains a type (1 byte) and a length (4 bytes)
                USP_LOG_Error("%s: Failed to parse incoming UDS Frame, as TLV is missing", __FUNCTION__);
                CloseUdsConnection(uc, RETRY_LATER);
                return;
            }

            if (uc->payload_length > MAX_UDS_FRAME_PAYLOAD_LEN)
            {
                // If a frame contains more than MAX_UDS_PAYLOAD_LEN of data then ignore it to limit memory usage
                USP_LOG_Error("%s: Incoming UDS frame is too large (%u bytes)", __FUNCTION__, uc->payload_length);
                CloseUdsConnection(uc, RETRY_LATER);
                return;
            }

            uc->rx_buf =  USP_MALLOC(uc->payload_length);
        }
    }

    if ((uc->hdr_bytes_rxed == 4) && (uc->len_bytes_rxed == 4))
    {
        // we have the header and the length, now read the payload
        num_bytes = recv(uc->socket, uc->rx_buf + uc->payload_bytes_rxed, uc->payload_length - uc->payload_bytes_rxed, 0);
        if (num_bytes <= 0) // -1 = error, 0 = other end closed
        {
            USP_LOG_Warning("%s: Endpoint (%s) disconnected from %s", __FUNCTION__, EndpointIdForLog(uc), uc->socket_path);
            CloseUdsConnection(uc, RETRY_LATER);
            return;
        }
        uc->payload_bytes_rxed += num_bytes;

        if (uc->payload_bytes_rxed == uc->payload_length)
        {
            ProcessUdsFrame(uc);

            // reset xxxBytesReceived to start parsing the next frame
            uc->hdr_bytes_rxed = 0;
            uc->len_bytes_rxed = 0;
            uc->payload_bytes_rxed = 0;
            USP_FREE(uc->rx_buf);
            uc->rx_buf = NULL;
        }
    }
}

/*********************************************************************//**
**
** ProcessUdsFrame
**
** Processes the payload of a UDS frame
**
** \param   uc - UDS connection which received some data
**
** \return  None
**
**************************************************************************/
void ProcessUdsFrame(uds_connection_t *uc)
{
    int err;
    unsigned char *p;
    int payload_remaining;
    int tlv_type;
    int tlv_len;
    char err_msg[128];
    char *saved_endpoint = NULL;
    char *saved_password = NULL;

    // Exit if the payload was invalid
    err_msg[0] = '\0';
    err = ValidateUdsPayload(uc->rx_buf, uc->payload_length);
    if (err != USP_ERR_OK)
    {
        USP_SNPRINTF(err_msg, sizeof(err_msg), "%s: Failed to parse incoming UDS frame", __FUNCTION__);
        goto exit;
    }

    // Iterate over all the TLVs in the payload, processing them
    p = uc->rx_buf;
    payload_remaining = uc->payload_length;
    while (payload_remaining > 0)
    {
        tlv_type = READ_BYTE(p, payload_remaining);
        tlv_len = READ_4_BYTES(p, payload_remaining);

        switch(tlv_type)
        {
            case kUdsFrameType_Handshake:
                if (saved_endpoint != NULL)
                {
                    USP_SNPRINTF(err_msg, sizeof(err_msg), "%s: Frame contains multiple Handshake TLVs", __FUNCTION__);
                    goto exit;
                }
                saved_endpoint = StrDupUdsTlvPayload(p, tlv_len);
                break;

            case kUdsFrameType_Error:
                ProcessUdsTLV_Error(uc, p, tlv_len);
                break;

            case kUdsFrameType_UspRecord:
                ProcessUdsTLV_UspRecord(uc, p, tlv_len);
                break;

            case kUdsFrameType_Password:
                if (saved_password != NULL)
                {
                    USP_SNPRINTF(err_msg, sizeof(err_msg), "%s: Frame contains multiple Password TLVs", __FUNCTION__);
                    goto exit;
                }
                saved_password = StrDupUdsTlvPayload(p, tlv_len);
                break;

            default:
                // Unexpected type in the USP TLV, ignoring the frame as per R-UDS.15
                USP_LOG_Warning("%s: Unsupported UDS TLV type (0x%02x). Ignoring packet", __FUNCTION__, tlv_type);
                break;
        }

        // Skip TLV payload, moving to next TLV
        SKIP_N_BYTES(p, tlv_len, payload_remaining);
    }

    // Process Handshake frame (containing endpoint_id and optionally password)
    if ((saved_endpoint != NULL) || (saved_password != NULL))
    {
        ProcessUdsTLV_HandshakePassword(uc, saved_endpoint, saved_password);
    }

exit:
    // Send an error frame if an error was detected
    if (err_msg[0] != '\0')
    {
        USP_LOG_Error("%s", err_msg);
        SendUdsErrorFrame(uc, err_msg);
    }

    // Free memory allocated by this function
    USP_SAFE_FREE(saved_endpoint);
    USP_SAFE_FREE(saved_password);

#ifdef FD_PASSING_EXPERIMENTAL
    FD_VECTOR_Close(uc->fd_buffer, uc->fd_count);
    uc->fd_buffer = NULL;
    uc->fd_count = 0;
#endif
}

/*********************************************************************//**
**
** SendUdsErrorFrame
**
** Private internal function to send a UDS error frame
**
** \param   uc - Pointer to uds_connection_t instance structure
**
** \param   errorString - error message describing the failure
**
** \return  None
**
**************************************************************************/
void SendUdsErrorFrame(uds_connection_t *uc, char* errorString)
{
    mtp_send_item_t msi;
    mtp_conn_t mtp_conn;

    msi.usp_msg_type = INVALID_USP_MSG_TYPE; //USP message type is for logging purpose only
    msi.content_type = kMtpContentType_UspMessage;
    msi.pbuf_len = strlen(errorString);
    msi.pbuf = USP_MALLOC(msi.pbuf_len);
    memcpy(msi.pbuf, errorString, msi.pbuf_len);
    PopulateUdsMtpConnection(&mtp_conn, uc);
#ifdef FD_PASSING_EXPERIMENTAL
    msi.fd_key = 0;
#endif

    UDS_QueueBinaryMessage(&msi, &mtp_conn, END_OF_TIME, kUdsFrameType_Error);
}

/*********************************************************************//**
**
** ValidateUdsEndpointID
**
** Private internal function to validate EndpointID from a received UDS handshake
**
** \param   endpointID - endpointID string
**
** \param   path_type - whether the endpoint is connected to the Broker's Controller or the Broker's Agent socket
**
** \return  a NULL if endpointID is valid or error string if endpointID is not valid
**
**************************************************************************/
char *ValidateUdsEndpointID(char* endpointID, uds_path_t path_type)
{
    int i;
    int count;
    uds_connection_t *uc;
    char *our_endpoint_id;

    if ((strcmp(endpointID, "") == 0) || (strcmp(endpointID, " ") == 0))
    {
        return "NULL or empty string in EndpointID, Failed to process Handshake Frame";
    }

    // Determine if EndpointID contains two colons
    count = 0;
    for (i=0; endpointID[i] != '\0'; i++)
    {
        if(endpointID[i] ==':')
        {
            count++;
        }
    }

    // Exit if EndpointID does not contain two colons
    if (count!=2)
    {
        return "Incorrect format of EndpointID, Failed to process Handshake Frame";
    }

    // Iterate over existing UDS connections, exiting if this endpoint_id is already connected on this UDS path type
    for (i=0; i<NUM_ELEM(uds_connections); i++)
    {

        uc = &uds_connections[i];
        if ((uc->instance != INVALID) && (uc->endpoint_id != NULL) &&
            (uc->path_type == path_type) && (strcmp(endpointID, uc->endpoint_id) == 0))
        {
            USP_LOG_Info("%s: Found matching path type %s and endpoint ID %s in existing connections", __FUNCTION__, UDS_PathTypeToString(path_type), endpointID);
            return "Duplicate EndpointID connecting on same UDS path, Failed to process Handshake Frame";
        }
    }

    // Disallow connections between endpoints with the same Endpoint ID
    our_endpoint_id = DEVICE_LOCAL_AGENT_GetEndpointID();
    if (strcmp(endpointID, our_endpoint_id)==0)
    {
        return "Connecting EndpointID is the same as this Endpoint, Failed to process Handshake Frame";
    }

    return NULL;
}

/*********************************************************************//**
**
** SendUdsFrames
**
** Checks the connection message queue and writes any data available to the socket
**
** \param   uc - Pointer to uds_connection_t instance structure
**
** \return  None
**
**************************************************************************/
void SendUdsFrames(uds_connection_t *uc)
{
#ifdef FD_PASSING_EXPERIMENTAL
    int bytes_sent = 0;
    unsigned int fd_key = 0;
    int fd_count = 0;
    int *fd_buffer = NULL;

    if (uc->socket == INVALID)
    {
        // bad socket - shouldn't really get here unless the socket was closed
        USP_LOG_Error("%s: transmit data on invalid socket", __FUNCTION__);
        return;
    }

    // Get the next frame, if not currently transmitting one
    if (uc->tx_buf == NULL)
    {
        fd_key = PopUdsSendItem(uc);

        // Exit if there's no more frames to send
        if (uc->tx_buf == NULL)
        {
            return;
        }
    }

    // if we get here there has to be data in uc->tx_buf (either partial remaining frame, or a newly constructed frame)
    USP_ASSERT(uc->tx_buf != NULL);

    // Try sending the remaining data in the UDS frame
    bytes_sent = sendmsg(uc->socket, uc->tx_buf, 0);
    if (fd_key != 0)
    {
        // If no references left for this buffer (no notifications or subscriptions holding reference)
        // it is need to be removed and closed
        if (FD_VECTOR_DecRef(fd_key) <= 0)
        {
            fd_buffer = FD_VECTOR_Get(fd_key, &fd_count);
            FD_VECTOR_Close(fd_buffer, fd_count);
            FD_VECTOR_Remove(fd_key);
        }
    }

    if (bytes_sent == -1)
    {
        USP_ERR_ERRNO("send", errno);
        CloseUdsConnection(uc, RETRY_LATER);
        return;
    }

    if (bytes_sent == uc->tx_buf_len)
    {
        // if we sent all the data then free the buffer.
        // The next record in the queue will be popped on the next iteration
        // If the UDS frame sent was of Error Type, close the connection
        // Close the uds connection, If the UDS frame sent was for usp disconnect record.
        if ((uc->tx_buf_type == kUdsFrameType_Error) || (uc->is_disconnect_record))
        {
            CloseUdsConnection(uc, RETRY_LATER);
        }

        FreeCurUdsTxItem(uc);
        uc->is_disconnect_record = false;
    }
#else
    int bytes_sent = 0;

    if (uc->socket == INVALID)
    {
        // bad socket - shouldn't really get here unless the socket was closed
        USP_LOG_Error("%s: transmit data on invalid socket", __FUNCTION__);
        return;
    }

    // Get the next frame, if not currently transmitting one
    if (uc->tx_buf == NULL)
    {
        // Exit if there's no more frames to send
        PopUdsSendItem(uc);
        if (uc->tx_buf == NULL)
        {
            return;
        }
    }

    // if we get here there has to be data in uc->tx_buf (either partial remaining frame, or a newly constructed frame)
    USP_ASSERT(uc->tx_buf != NULL);

    // Try sending the remaining data in the UDS frame
    bytes_sent = send(uc->socket, &uc->tx_buf[uc->tx_bytes_sent], (uc->tx_buf_len - uc->tx_bytes_sent), 0);
    if (bytes_sent == -1)
    {
        USP_ERR_ERRNO("send", errno);
        CloseUdsConnection(uc, RETRY_LATER);
        return;
    }

    uc->tx_bytes_sent += bytes_sent;
    if (uc->tx_bytes_sent == uc->tx_buf_len)
    {
        // if we sent all the data then free the buffer.
        // The next record in the queue will be popped on the next iteration
        // If the UDS frame sent was of Error Type, close the connection
        // Close the uds connection, If the UDS frame sent was for usp disconnect record.
        if ((uc->tx_buf_type == kUdsFrameType_Error) || (uc->is_disconnect_record))
        {
            CloseUdsConnection(uc, RETRY_LATER);
        }

        FreeCurUdsTxItem(uc);
        uc->is_disconnect_record = false;
    }
#endif
}

/*********************************************************************//**
**
** PopUdsSendItem
**
** Removes the first item, forms a UDS frame with it, and puts the frame into the outgoing buffer, pending sending
**
** \param   uc - Pointer to uds_connection_t instance structure
**
** \return  (only used by FD_PASSING_EXPERIMENTAL) key for file descriptor buffer in global map
**
**************************************************************************/
#ifdef FD_PASSING_EXPERIMENTAL
unsigned int PopUdsSendItem(uds_connection_t *uc)
{
    int tlv_len = 0;
    char *endpoint_id = "";  // NOTE: This is initialised to an empty string to prevent Clang static analyser from generating a false positive if it was initialised to NULL
    int endpoint_len = 0;
    unsigned char *p;
    int password_len = 0;
    bool include_self_password = false;
    struct msghdr *bmsg = NULL;
    struct cmsghdr* cmsg;
    int *fd_buffer = NULL;
    bool success = false;
    unsigned int fd_key = 0;
    int fd_count = 0;

    USP_ASSERT(uc->tx_buf == NULL);

    //Remove any queued messages that have expired
    RemoveExpiredUdsMessages(uc);

    // Exit if no more USP records left to send
    uds_send_item_t *send_item = (uds_send_item_t *) uc->usp_record_send_queue.head;
    if (send_item == NULL)
    {
        return 0;
    }

    // Calculate the length of the TLV payload
    switch (send_item->type)
    {
        case kUdsFrameType_UspRecord:
            // Log the USP Record before we send the first chunk
            MSG_HANDLER_LogMessageToSend(&send_item->item, kMtpProtocol_UDS, uc->endpoint_id, NULL);
            tlv_len = TLV_HEADER_SIZE + send_item->item.pbuf_len;

            if (send_item->item.fd_key != 0)
            {
                fd_buffer = FD_VECTOR_Get(send_item->item.fd_key, &fd_count);
                fd_key = send_item->item.fd_key;
            }
            break;

        case kUdsFrameType_Error:
            tlv_len = TLV_HEADER_SIZE + send_item->item.pbuf_len;
            break;

        case kUdsFrameType_Handshake:
            endpoint_id = DEVICE_LOCAL_AGENT_GetEndpointID();
            USP_ASSERT(endpoint_id != NULL);
            endpoint_len = strlen(endpoint_id);
            tlv_len = TLV_HEADER_SIZE + endpoint_len;

            // Optionally add password TLV
            include_self_password = (uds_self_password != NULL) && (*uds_self_password != '\0');
            if (include_self_password)
            {
                password_len = strlen(uds_self_password);
                tlv_len += TLV_HEADER_SIZE + password_len;
            }
            break;


        default:
            TERMINATE_BAD_CASE(send_item->type);
            break;
    }

    if (CMSG_SPACE(sizeof(int) * fd_count) > sizeof(struct cmsghdr))
    {
        cmsg = USP_MALLOC(CMSG_SPACE(sizeof(int) * fd_count));
    }
    else
    {
        cmsg = USP_MALLOC(sizeof(struct cmsghdr));
    }

    bmsg = USP_MALLOC(sizeof(struct msghdr));
    if (bmsg == NULL)
    {
        goto exit;
    }

    memset(bmsg, 0, sizeof(struct msghdr));

    // frame consists of 4 sync bytes + 4 length bytes + payload
    uc->tx_buf_len = sizeof(uds_frame_sync_bytes) + 4 + tlv_len;
    p = USP_MALLOC(uc->tx_buf_len);
    if (p == NULL)
    {
        goto exit;
    }

    bmsg->msg_iov = USP_MALLOC(sizeof(struct iovec));
    bmsg->msg_iov->iov_base = p;
    bmsg->msg_iov->iov_len = uc->tx_buf_len;
    bmsg->msg_iovlen = 1;
    bmsg->msg_control = cmsg;
    bmsg->msg_controllen = CMSG_SPACE(sizeof(int) * fd_count);

    cmsg->cmsg_level = SOL_SOCKET;
    cmsg->cmsg_type = SCM_RIGHTS;
    cmsg->cmsg_len = CMSG_LEN(sizeof(int) * fd_count);
    if (fd_buffer)
    {
        memcpy(CMSG_DATA(cmsg), fd_buffer, sizeof(int) * fd_count);
    }

    uc->tx_buf = bmsg;
#else

void PopUdsSendItem(uds_connection_t *uc)
{
    int tlv_len = 0;
    char *endpoint_id = "";  // NOTE: This is initialised to an empty string to prevent Clang static analyser from generating a false positive if it was initialised to NULL
    int endpoint_len = 0;
    unsigned char *p;
    int password_len = 0;
    bool include_self_password = false;

    USP_ASSERT(uc->tx_buf == NULL);

    //Remove any queued messages that have expired
    RemoveExpiredUdsMessages(uc);

    // Exit if no more USP records left to send
    uds_send_item_t *send_item = (uds_send_item_t *) uc->usp_record_send_queue.head;
    if (send_item == NULL)
    {
        return;
    }

    // Calculate the length of the TLV payload
    switch (send_item->type)
    {
        case kUdsFrameType_UspRecord:
            // Log the USP Record before we send the first chunk
            MSG_HANDLER_LogMessageToSend(&send_item->item, kMtpProtocol_UDS, uc->endpoint_id, NULL);
            tlv_len = TLV_HEADER_SIZE + send_item->item.pbuf_len;
            break;

        case kUdsFrameType_Error:
            tlv_len = TLV_HEADER_SIZE + send_item->item.pbuf_len;
            break;

        case kUdsFrameType_Handshake:
            endpoint_id = DEVICE_LOCAL_AGENT_GetEndpointID();
            USP_ASSERT(endpoint_id != NULL);
            endpoint_len = strlen(endpoint_id);
            tlv_len = TLV_HEADER_SIZE + endpoint_len;

            // Optionally add password TLV
            include_self_password = (uds_self_password != NULL) && (*uds_self_password != '\0');
            if (include_self_password)
            {
                password_len = strlen(uds_self_password);
                tlv_len += TLV_HEADER_SIZE + password_len;
            }
            break;

        default:
            TERMINATE_BAD_CASE(send_item->type);
            break;
    }

    // frame consists of 4 sync bytes + 4 length bytes + payload
    uc->tx_buf_len = sizeof(uds_frame_sync_bytes) + 4 + tlv_len;
    uc->tx_buf = USP_MALLOC(uc->tx_buf_len);
    uc->tx_bytes_sent = 0;

    p = uc->tx_buf;
#endif

    // Construct frame header
    WRITE_N_BYTES(p, uds_frame_sync_bytes, sizeof(uds_frame_sync_bytes));
    WRITE_4_BYTES(p, tlv_len);

    // construct the USP type/len/value
    switch (send_item->type)
    {
        case kUdsFrameType_UspRecord:
            USP_LOG_Info("Sending USP RECORD to endpoint_id=%s on %s", EndpointIdForLog(uc), uc->socket_path);
            WRITE_BYTE(p, kUdsFrameType_UspRecord);
            WRITE_4_BYTES(p, send_item->item.pbuf_len);
            WRITE_N_BYTES(p, send_item->item.pbuf, send_item->item.pbuf_len);
            USP_FREE(send_item->item.pbuf);
            break;

        case kUdsFrameType_Error:
            USP_LOG_Info("Sending UDS ERROR to endpoint_id=%s on %s", EndpointIdForLog(uc), uc->socket_path);
            WRITE_BYTE(p, kUdsFrameType_Error);
            WRITE_4_BYTES(p, send_item->item.pbuf_len);
            WRITE_N_BYTES(p, send_item->item.pbuf, send_item->item.pbuf_len);
            USP_FREE(send_item->item.pbuf);
            break;

        case kUdsFrameType_Handshake:
            USP_LOG_Info("Sending UDS HANDSHAKE to endpoint_id=%s on %s", EndpointIdForLog(uc), uc->socket_path);
            WRITE_BYTE(p, kUdsFrameType_Handshake);
            WRITE_4_BYTES(p, endpoint_len);
            WRITE_N_BYTES(p, endpoint_id, endpoint_len);

            // Optionally add password TLV
            if (include_self_password)
            {
                WRITE_BYTE(p, kUdsFrameType_Password);
                WRITE_4_BYTES(p, password_len);
                WRITE_N_BYTES(p, uds_self_password, password_len);
            }
            break;


        default:
            TERMINATE_BAD_CASE(send_item->type);
            break;
    }

    //Update the type of frame being sent. This will be needed to close connection, if the frame being sent is of type Error
    uc->tx_buf_type = send_item->type;
    //Update if the usp message being sent is of disconnect record type. This will be needed to close connection, if disconnect record is being sent
    uc->is_disconnect_record = (send_item->item.content_type == kMtpContentType_DisconnectRecord) ? true : false;

    // now we've converted the send item into a UDS frame
    // free up the record and remove it from the tx queue
    DLLIST_Unlink(&uc->usp_record_send_queue, send_item);
    USP_FREE(send_item);

#ifdef FD_PASSING_EXPERIMENTAL
    success = true;

exit:
    if (!success)
    {
        if (bmsg != NULL)
        {
            USP_SAFE_FREE(bmsg->msg_iov);
        }
        USP_SAFE_FREE(bmsg);
        USP_SAFE_FREE(p);
    }

    return fd_key;
#endif
}

/*********************************************************************//**
**
** FreeCurUdsTxItem
**
** Frees the memory associated with the current item being transmitted (if not already freed)
**
** \param   uc - Pointer to the connection to free the current TX item from
**
** \return  None
**
**************************************************************************/
void FreeCurUdsTxItem(uds_connection_t *uc)
{
    // Exit if the memory has already been freed (nothing to do)
    if (uc->tx_buf == NULL)
    {
        return;
    }

#ifdef FD_PASSING_EXPERIMENTAL
    if (uc->tx_buf->msg_iov != NULL)
    {
        USP_SAFE_FREE(uc->tx_buf->msg_iov->iov_base);
        USP_FREE(uc->tx_buf->msg_iov);
        uc->tx_buf->msg_iov = NULL;
    }

    USP_SAFE_FREE(uc->tx_buf->msg_control);
#endif

    USP_FREE(uc->tx_buf);
    uc->tx_buf = NULL;
}

/*********************************************************************//**
**
** HandleUdsListeningSocketConnection
**
** Called when a client connects to one of our UDS listening sockets
**
** \param   us - UDS server which a client has connected to
**
** \return  USP_ERR_OK if successful
**
**************************************************************************/
int HandleUdsListeningSocketConnection(uds_server_t *us)
{
    USP_ASSERT(us);

    int socket;
    uds_connection_t *uc = NULL;
    struct sockaddr sa;
    socklen_t sa_len;

    // Exit if unable to accept the connection
    sa_len = sizeof(sa);
    socket = accept4(us->listen_sock, &sa, &sa_len, SOCK_NONBLOCK);
    if (socket == -1)
    {
        // If an error occurred, just log it
        USP_ERR_ERRNO("accept", errno);
        return USP_ERR_INTERNAL_ERROR;
    }

    // Exit if no free internal UDS connection entries
    uc = FindFreeUdsConnection();
    if (uc == NULL)
    {
        USP_LOG_Error("%s: Too many connections - ignoring connect request", __FUNCTION__);
        close (socket);
        return USP_ERR_INTERNAL_ERROR;
    }

    // Fill in the UDS connection structure
    InitialiseUdsConnection(uc);
    uc->type = kUdsConnType_Server;
    uc->socket = socket;
    uc->conn_id = CalcNextUdsConnectionId();
    uc->instance = us->instance;
    uc->path_type = us->path_type;
    uc->auth_required = us->auth_required;
    uc->socket_path = USP_STRDUP(us->socket_path);

    DLLIST_Init(&uc->usp_record_send_queue);

    // NOTE: The UDS handshake record is sent in response to the handshake message received from the client [R-UDS.17], rather than here

    return USP_ERR_OK;
}

/*********************************************************************//**
**
** CalcNextUdsConnectionId
**
** Assigns a unique identifier for a UDS connection
**
** \param   None
**
** \return  unique identifier
**
**************************************************************************/
unsigned CalcNextUdsConnectionId(void)
{
    uds_conn_id_counter++;

    // Ensure that counter is unique, after unsigned wrap around
    while (FindUdsConnectionByConnId(uds_conn_id_counter) != NULL)
    {
        uds_conn_id_counter++;
    }

    return uds_conn_id_counter;
}

/*********************************************************************//**
**
** RemoveExpiredUdsMessages
**
** Removes all expired messages from the queue of messages to send
** NOTE: This mechanism can be used to prevent the queue from filling up needlessly if the controller is offine
**
** \param   uc - pointer to UDS connection
**
** \return  None
**
**************************************************************************/
void RemoveExpiredUdsMessages(uds_connection_t *uc)
{
    time_t cur_time;
    uds_send_item_t *queued_msg;
    uds_send_item_t *next_msg;
    char cur_time_buf[MAX_ISO8601_LEN], exp_time_buf[MAX_ISO8601_LEN];

    cur_time = time(NULL);
    iso8601_from_unix_time(cur_time, cur_time_buf, sizeof(cur_time_buf));
    queued_msg = (uds_send_item_t *) uc->usp_record_send_queue.head;
    while (queued_msg != NULL)
    {
        next_msg = (uds_send_item_t *) queued_msg->link.next;
        if (cur_time > queued_msg->expiry_time)
        {
            iso8601_from_unix_time(queued_msg->expiry_time, exp_time_buf, sizeof(exp_time_buf));
            USP_LOG_Warning("Removing message from queue with expiry time = %s, current time = %s", exp_time_buf, cur_time_buf);
            RemoveUdsQueueItem(uc, queued_msg);
        }

        queued_msg = next_msg;
    }
}


/*********************************************************************//**
**
** RemoveUdsQueueItem
**
** Frees the specified item in the send queue of the specified controller
**
** \param   uc - pointer to UDS connection
** \param   queued_msg - message to remove from the queue
**
** \return  None
**
**************************************************************************/
void RemoveUdsQueueItem(uds_connection_t *uc, uds_send_item_t *queued_msg)
{
#ifdef FD_PASSING_EXPERIMENTAL
    if (queued_msg->item.fd_key > 0)
    {
        if (FD_VECTOR_DecRef(queued_msg->item.fd_key) <= 0)
        {
            int fd_count = 0;
            int* fd_buffer = FD_VECTOR_Get(queued_msg->item.fd_key, &fd_count);
            if ((fd_buffer != NULL) && (fd_count > 0))
            {
                FD_VECTOR_Close(fd_buffer, fd_count);
            }
        }
    }
#endif

    USP_ASSERT(queued_msg != NULL);

    // Free all dynamically allocated member variables
    USP_SAFE_FREE(queued_msg->item.pbuf);

    // Remove the specified item from the queue, and free the item itself
    DLLIST_Unlink(&uc->usp_record_send_queue, queued_msg);
    USP_FREE(queued_msg);
}

/*********************************************************************//**
**
** IsUspRecordInUdsQueue
**
** Determines whether the specified USP record is already queued, waiting to be sent
** This is used to avoid duplicate records being placed in the queue, which could occur under notification retry conditions
**
** \param   uc - uds connection which has USP records queued to send
** \param   pbuf - pointer to buffer containing USP Record to match against
** \param   pbuf_len - length of buffer containing USP Record to match against
**
** \return  true if the message is already queued
**
**************************************************************************/
bool IsUspRecordInUdsQueue(uds_connection_t *uc, unsigned char *pbuf, int pbuf_len)
{
    uds_send_item_t *queued_msg;

    // Iterate over USP Records in the STOMP queue
    queued_msg = (uds_send_item_t *) uc->usp_record_send_queue.head;
    while (queued_msg != NULL)
    {
        // Exit if the USP record is already in the queue
        if ((queued_msg->item.pbuf_len == pbuf_len) && (memcmp(queued_msg->item.pbuf, pbuf, pbuf_len)==0))
        {
             return true;
        }

        // Move to next message in the queue
        queued_msg = (uds_send_item_t *) queued_msg->link.next;
    }

    // If the code gets here, then the USP record is not in the queue
    return false;
}

/*********************************************************************//**
**
** FindUdsServerByInstanceId
**
** Private internal function to find a UDS server matching the datamodel instance
**
** \param   instance - the instance ID as specified in the datamodel
**
** \return  a pointer to the server, or NULL if none are found
**
**************************************************************************/
uds_server_t *FindUdsServerByInstanceId(int instance)
{
    int i;
    uds_server_t *us = NULL;

    for (i=0; i<NUM_ELEM(uds_servers); i++)
    {
        us = &uds_servers[i];
        if ((us->instance != INVALID) && (us->instance == instance))
        {
            return us;
        }
    }
    return NULL;
}

/*********************************************************************//**
**
** FindUdsClientByInstanceId
**
** Private internal function to find a UDS client connection matching the datamodel instance
**
** \param   instance - the instance ID as specified in the datamodel
**
** \return  a pointer to the connection, or NULL if none are found
**
**************************************************************************/
uds_connection_t *FindUdsClientByInstanceId(int instance)
{
    int i;
    uds_connection_t *uc = NULL;

    for (i=0; i<NUM_ELEM(uds_connections); i++)
    {
        uc = &uds_connections[i];
        if ((uc->instance != INVALID) && (uc->instance == instance))
        {
            // this is a client connection or a client connection pending a retry
            return uc;
        }
    }
    return NULL;
}

/*********************************************************************//**
**
** InitialiseUdsConnection
**
** Initialises uds_connection_t struct to default (invalid) values
**
** \param   pointer to a uds_connection_t structure
**
** \return  none
**
**************************************************************************/
void InitialiseUdsConnection(uds_connection_t *uc)
{
    memset(uc, 0, sizeof(uds_connection_t));
    uc->instance = INVALID;
    uc->type = kUdsConnType_Invalid;
    uc->path_type = kUdsPathType_Invalid;
    uc->auth_required = true;
    uc->socket = INVALID;
    uc->hdr_bytes_rxed = 0;
    uc->len_bytes_rxed = 0;
    uc->payload_bytes_rxed = 0;
    uc->payload_length = 0;
    uc->rx_buf = NULL;
    uc->endpoint_id = NULL;
    uc->usp_record_send_queue.head = NULL;
    uc->usp_record_send_queue.tail = NULL;
    uc->tx_buf = NULL;
#ifndef FD_PASSING_EXPERIMENTAL
    uc->tx_bytes_sent = 0;
#endif
    uc->tx_buf_len = 0;
    uc->tx_buf_type = kUdsFrameType_Invalid;
    uc->is_disconnect_record = false;
    uc->conn_id = INVALID;
    uc->socket_path = NULL;
    uc->retry_timeout = INVALID;
    uc->schedule_reconnect = kScheduledAction_Off;
    uc->next_conn_params.mode = kUdsConnType_Invalid;
    uc->next_conn_params.path_type = kUdsPathType_Invalid;
    uc->next_conn_params.instance = INVALID;
    uc->next_conn_params.path = NULL;
}

/*********************************************************************//**
**
** InitialiseUdsServer
**
** Initialises uds_server_t struct to default (invalid) values
**
** \param   pointer to a uds_server_t structure
**
** \return  none
**
**************************************************************************/
void InitialiseUdsServer(uds_server_t *us)
{
    memset(us, 0, sizeof(uds_server_t));
    us->listen_sock = INVALID;
    us->instance = INVALID;
    us->path_type = kUdsPathType_Invalid;
    us->schedule_reconnect = kScheduledAction_Off;
    us->next_conn_params.mode = kUdsConnType_Invalid;
    us->next_conn_params.path_type = kUdsPathType_Invalid;
    us->next_conn_params.instance = INVALID;
    us->next_conn_params.path = NULL;
    us->socket_path = NULL;
}

/*********************************************************************//**
**
** EndpointIdForLog
**
** Returns string of EndpointId to report in logs
** This function is necessary because the remote endpoint_id may be unknown at the time that the log statement needs to be printed
**
** \param   pointer to a uds_server_t structure
**
** \return  none
**
**************************************************************************/
char *EndpointIdForLog(uds_connection_t *uc)
{
    return (uc->endpoint_id == NULL) ? "UNKNOWN" : uc->endpoint_id;
}

/*********************************************************************//**
**
** PopulateUdsMtpConnection
**
** Helper function to copy the relevant fields from the UDS connection struture into the MTP connection structure
**
** \param   mtpc - structure to populate with details of the connection
** \param   uc - UDS Connection structure to copy the connection details from
**
** \return  instance number in Device.UnixDomainSockets.UnixDomainSocket.{i} or INVALID if the connection had died
**
**************************************************************************/
void PopulateUdsMtpConnection(mtp_conn_t *mtpc, uds_connection_t *uc)
{
    memset(mtpc, 0, sizeof(mtp_conn_t));
    mtpc->is_reply_to_specified = true;
    mtpc->protocol = kMtpProtocol_UDS;
    mtpc->uds.instance = uc->instance;
    mtpc->uds.conn_id = uc->conn_id;
    mtpc->uds.path_type = uc->path_type;
}

/*********************************************************************//**
**
** ValidateUdsPayload
**
** Validates that the payload of a UDS frame is correctly formed
** ie that the sum of the TLV headers and lengths matches the frame payload length
**
** \param   payload - pointer to buffer containing payload of UDS frame
** \param   payload_len - length of payload in UDS frame
**
** \return  USP_ERR_OK if valid
**
**************************************************************************/
int ValidateUdsPayload(unsigned char *payload, int payload_len)
{
    unsigned char *p;
    int payload_remaining;
    int tlv_len;

    // Iterate over all the TLVs in the payload, validating their length
    p = payload;
    payload_remaining = payload_len;
    while (payload_remaining > 0)
    {
        // Exit if payload is not large enough to contain TLV header
        if (payload_remaining < TLV_HEADER_SIZE)
        {
            return USP_ERR_RECORD_NOT_PARSED;
        }

        SKIP_BYTE(p, payload_remaining);  // Skip TLV type byte
        tlv_len = READ_4_BYTES(p, payload_remaining);

        // Exit if the TLV's length is invalid
        if ((tlv_len < 0) || (tlv_len > payload_remaining))
        {
            return USP_ERR_RECORD_NOT_PARSED;
        }

        // Skip TLV payload, moving to next TLV
        SKIP_N_BYTES(p, tlv_len, payload_remaining);
    }

    return USP_ERR_OK;
}

/*********************************************************************//**
**
** StrDupUdsTlvPayload
**
** Copies the specified TLV payload into a dynamically allocated string
**
** \param   tlv_payload - Pointer to the start of the TLV's payload
** \param   tlv_len - The length of the TLV's payload
**
** \return  USP_ERR_OK if valid
**
**************************************************************************/
char *StrDupUdsTlvPayload(unsigned char *tlv_payload, int tlv_len)
{
    char *buf;

    buf = USP_MALLOC(tlv_len+1);   // Plus 1 to include NULL terminator
    memcpy(buf, tlv_payload, tlv_len);
    buf[tlv_len] = '\0';

    return buf;
}

/*********************************************************************//**
**
** ProcessUdsTLV_Error
**
** Process the payload of an Error TLV
**
** \param   uc - UDS connection which received some data
** \param   tlv_payload - Pointer to the start of the TLV's payload
** \param   tlv_len - The length of the TLV's payload
**
** \return  None
**
**************************************************************************/
void ProcessUdsTLV_Error(uds_connection_t *uc, unsigned char *tlv_payload, unsigned tlv_len)
{
    char buf[128];
    unsigned len;

    // Truncate the error message
    len = MIN(tlv_len, (sizeof(buf)-1));
    memcpy(buf, (char*)tlv_payload, len);
    buf[len] = '\0'; // NULL terminate the error string

    // Log the error message
    USP_LOG_Error("Received UDS ERROR from endpoint_id=%s on %s", EndpointIdForLog(uc), uc->socket_path);
    USP_LOG_Error("UDS ERROR is '%s'", buf);

#ifdef FD_PASSING_EXPERIMENTAL
    if (uc->fd_count != 0)
    {
        SendUdsErrorFrame(uc, "File descriptors are not supported with error frame");
    }
#endif

    // Close the uds socket connection
    CloseUdsConnection(uc, RETRY_LATER);
}

/*********************************************************************//**
**
** ProcessUdsTLV_UspRecord
**
** Process the payload of a TLV containing a USP Record
**
** \param   uc - UDS connection which received some data
** \param   tlv_payload - Pointer to the start of the TLV's payload
** \param   tlv_len - The length of the TLV's payload
**
** \return  None
**
**************************************************************************/
void ProcessUdsTLV_UspRecord(uds_connection_t *uc, unsigned char *tlv_payload, unsigned tlv_len)
{
    char time_buf[MAX_ISO8601_LEN];
    mtp_conn_t mtp_conn;
    int role_instance;

    // Exit, discarding received frame if we received a USP Record before the handshake process has completed [R-UDS.20]
    if (uc->endpoint_id == NULL)
    {
        USP_LOG_Warning("%s: Ignoring USP frame received before handshake completed", __FUNCTION__);
        return;
    }

    iso8601_cur_time(time_buf, sizeof(time_buf));
    USP_LOG_Info("USP Record received at time %s, from endpoint_id=%s over UDS (%s)", time_buf, EndpointIdForLog(uc), uc->socket_path);

    PopulateUdsMtpConnection(&mtp_conn, uc);
    role_instance = (uc->auth_required) ? ROLE_UDS_AUTH : ROLE_UDS;

#ifdef FD_PASSING_EXPERIMENTAL
    unsigned int fd_key = 0;
    if (uc->fd_count != 0)
    {
        fd_key = FD_VECTOR_New_Key();
        if (fd_key == 0)
        {
            mtp_conn.uds.fd_res_exceeded = true;
        }
        else
        {
            mtp_conn.uds.fd_key = fd_key;
            FD_VECTOR_Add(mtp_conn.uds.fd_key, uc->fd_buffer, uc->fd_count);
            uc->fd_buffer = NULL;
            uc->fd_count = 0;
        }
    }
#endif

    DM_EXEC_PostUspRecord(tlv_payload, tlv_len, uc->endpoint_id, role_instance, &mtp_conn);
}

/*********************************************************************//**
**
** ProcessUdsTLV_HandshakePassword
**
** Processes the contents of a UDS frame that contained a handshake TLV and optionally a password TLV
**
** \param   uc - UDS connection which received some data
** \param   endpoint_id - Endpoint ID of the peer, extracted from the Handshake TLV, or NULL if Handshake TLV not present in UDS frame
** \param   password - Password provided by the peer, extracted from the Password TLV, or NULL if Password TLV not present in UDS frame
**
** \return  None
**
**************************************************************************/
void ProcessUdsTLV_HandshakePassword(uds_connection_t *uc, char *endpoint_id, char *password)
{
    char *err_msg = NULL;
    char *expected_password;
    int index;

    // Exit if we've already received a successful handshake (in which case, we just ignore this handshake packet [R-UDS.19])
    if (uc->endpoint_id != NULL)
    {
        USP_LOG_Warning("%s: Ignoring extraneous UDS MTP handshake", __FUNCTION__);
        return;
    }

    // Exit if no EndpointID
    if ((endpoint_id == NULL) || (*endpoint_id == '\0'))
    {
        // R-UDS.21 : UDS client or server must send error frame if it cannot parse handshake frame
        err_msg = "Invalid EndpointID, Failed to process Handshake Frame";
        USP_LOG_Error("%s: %s", __FUNCTION__, err_msg);
        SendUdsErrorFrame(uc, err_msg);
        return;
    }

    // Exit if peer's endpoint_id was badly formed, or already connected to this UDS path type
    err_msg = ValidateUdsEndpointID(endpoint_id, uc->path_type);
    if (err_msg != NULL)
    {
        // R-UDS.21 : UDS client or server must send error frame if it cannot process the handshake
        USP_LOG_Error("%s: %s", __FUNCTION__, err_msg);
        SendUdsErrorFrame(uc, err_msg);
        return;
    }

    // Handle connections that require password based authentication
    if (uc->auth_required)
    {
        // Exit if the expected password was blank, in which case the USP Service is blocked from connecting with any password
        index = KV_VECTOR_FindKey(&uds_passwords, endpoint_id, 0);
        if ((index != INVALID) && (uds_passwords.vector[index].value[0] == '\0'))
        {
            err_msg = "USP Service blocked";
            USP_LOG_Error("%s: %s", __FUNCTION__, err_msg);
            SendUdsErrorFrame(uc, err_msg);
            return;
        }

        // Exit if a password was required, but none provided
        if (password == NULL)
        {
            err_msg = "Password required but none provided in UDS frame";
            USP_LOG_Error("%s: %s", __FUNCTION__, err_msg);
            SendUdsErrorFrame(uc, err_msg);
            return;
        }

        // Exit if no authentication setup for this endpoint
        if (index == INVALID)
        {
            err_msg = "Password required but none configured";
            USP_LOG_Error("%s: %s for endpoint_id=%s", __FUNCTION__, err_msg, endpoint_id);
            SendUdsErrorFrame(uc, err_msg);
            return;
        }

        // Exit if password was incorrect
        expected_password = uds_passwords.vector[index].value;
        if (strcmp(password, expected_password) != 0)
        {
            err_msg = "Incorrect Password";
            USP_LOG_Error("%s: %s for endpoint_id=%s", __FUNCTION__, err_msg, endpoint_id);
            SendUdsErrorFrame(uc, err_msg);
            return;
        }
    }

    // Save the endpoint_id of the connecting client
    uc->endpoint_id = USP_STRDUP(endpoint_id);
    USP_LOG_Info("Received UDS HANDSHAKE from endpoint_id=%s on %s", EndpointIdForLog(uc), uc->socket_path);

    if ( ((RUNNING_AS_USP_SERVICE()==true) && (uc->path_type == kUdsPathType_BrokersController)) ||
         ((RUNNING_AS_USP_SERVICE()==false) && (uc->path_type == kUdsPathType_BrokersAgent)) )
    {
        QueueUspConnectRecord_Uds(uc);
    }

    // cancel the handshake timeout during handshake exchange
    // Only the client sets a handshake timeout as only the client expects a handshake response
    // doing this on a server connection is benign
    uc->handshake_timeout = INVALID;

    // Notify the data model of the endpoint which has connected
    DM_EXEC_PostUdsHandshakeComplete(uc->endpoint_id, uc->instance, uc->path_type, uc->conn_id);

    // If acting as a server, then queue our UDS handshake message now, in response to the handshake message received from the connecting client [R-UDS.17]
    if (uc->type == kUdsConnType_Server)
    {
        uds_send_item_t *send_item;

        send_item = USP_MALLOC(sizeof(uds_send_item_t));
        memset(send_item, 0, sizeof(uds_send_item_t));
        send_item->expiry_time = END_OF_TIME;
        send_item->type = kUdsFrameType_Handshake;
        DLLIST_LinkToHead(&uc->usp_record_send_queue, send_item);
    }

#ifdef FD_PASSING_EXPERIMENTAL
    if (uc->fd_count != 0)
    {
        SendUdsErrorFrame(uc, "File descriptors are not supported with handshake frame");
    }
#endif
}

/*********************************************************************//**
**
** CopyUdsConnParams
**
** Performs a deep copy of UDS connection parameters
**
** \param   dest - pointer to destination structure to copy into
** \param   src - pointer to source structure to copy from
**
** \return  None
**
**************************************************************************/
void CopyUdsConnParams(uds_conn_params_t *dest, uds_conn_params_t *src)
{
    memset(dest, 0, sizeof(uds_conn_params_t));
    dest->instance = src->instance;
    dest->path = USP_STRDUP(src->path);
    dest->path_type = src->path_type;
    dest->mode = src->mode;
    dest->auth_required = src->auth_required;
    dest->registration_restricted = src->registration_restricted;
}

/*********************************************************************//**
**
** FreeUdsConnParams
**
** Safely frees all dynamically allocated memory associated with the UDS connection parameter structure
**
** \param   ucp - pointer to structure to free all dynamically allocated memory variables in
**
** \return  None
**
**************************************************************************/
void FreeUdsConnParams(uds_conn_params_t *ucp)
{
    USP_SAFE_FREE(ucp->path);
}
#endif // ENABLE_UDS
