mip_cmdqueue.c

Go to the documentation of this file.

#include "mip_cmdqueue.h"

#include "mip_field.h"
#include "mip_packet.h"

#include <string.h>
#include <assert.h>


#define MIP_REPLY_DESC_GLOBAL_ACK_NACK 0xF1

#define MIP_INDEX_REPLY_DESCRIPTOR 0
#define MIP_INDEX_REPLY_ACK_CODE   1


void mip_pending_cmd_init(mip_pending_cmd* cmd, uint8_t descriptor_set, uint8_t field_descriptor)
{
    mip_pending_cmd_init_full(cmd, descriptor_set, field_descriptor, 0x00, NULL, 0, 0);
}

void mip_pending_cmd_init_with_timeout(mip_pending_cmd* cmd, uint8_t descriptor_set, uint8_t field_descriptor, timeout_type additional_time)
{
    mip_pending_cmd_init_full(cmd, descriptor_set, field_descriptor, 0x00, NULL, 0, additional_time);
}

void mip_pending_cmd_init_with_response(mip_pending_cmd* cmd, uint8_t descriptor_set, uint8_t field_descriptor, uint8_t response_descriptor, uint8_t* response_buffer, uint8_t response_buffer_size)
{
    mip_pending_cmd_init_full(cmd, descriptor_set, field_descriptor, response_descriptor, response_buffer, response_buffer_size, 0);
}

void mip_pending_cmd_init_full(mip_pending_cmd* cmd, uint8_t descriptor_set, uint8_t field_descriptor, uint8_t response_descriptor, uint8_t* response_buffer, uint8_t response_buffer_size, timeout_type additional_time)
{
    cmd->_next                 = NULL;
    cmd->_response_buffer      = NULL;
    cmd->_extra_timeout        = additional_time;
    cmd->_descriptor_set       = descriptor_set;
    cmd->_field_descriptor     = field_descriptor;
    cmd->_response_descriptor  = response_descriptor;
    cmd->_response_buffer      = response_buffer;
    cmd->_response_buffer_size = response_buffer_size;
    // cmd->_ack_code            = 0xFF; // invalid
    cmd->_status               = MIP_STATUS_NONE;
}


enum mip_cmd_result mip_pending_cmd_status(const mip_pending_cmd* cmd)
{
    return cmd->_status;
}

const uint8_t* mip_pending_cmd_response(const mip_pending_cmd* cmd)
{
    assert(mip_cmd_result_is_finished(cmd->_status));

    return cmd->_response_buffer;
}

uint8_t mip_pending_cmd_response_length(const mip_pending_cmd* cmd)
{
    assert(mip_cmd_result_is_finished(cmd->_status));

    return cmd->_response_length;
}

int mip_pending_cmd_remaining_time(const mip_pending_cmd* cmd, timestamp_type now)
{
    assert(cmd->_status == MIP_STATUS_WAITING);

    // result <= 0 if timed out.
    // Note: this still works with unsigned overflow.
    return (int)(now - cmd->_timeout_time);
}

bool mip_pending_cmd_check_timeout(const mip_pending_cmd* cmd, timestamp_type now)
{
    if( cmd->_status == MIP_STATUS_WAITING )
    {
        if( mip_pending_cmd_remaining_time(cmd, now) > 0 )
        {
            return true;
        }
    }

    return false;
}


void mip_cmd_queue_init(mip_cmd_queue* queue, timeout_type base_reply_timeout)
{
    queue->_first_pending_cmd = NULL;
    queue->_base_timeout = base_reply_timeout;

    MIP_DIAG_ZERO(queue->_diag_cmds_queued);
    MIP_DIAG_ZERO(queue->_diag_cmds_acked);
    MIP_DIAG_ZERO(queue->_diag_cmds_nacked);
    MIP_DIAG_ZERO(queue->_diag_cmds_timedout);
    MIP_DIAG_ZERO(queue->_diag_cmds_failed);
}

void mip_cmd_queue_enqueue(mip_cmd_queue* queue, mip_pending_cmd* cmd)
{
    // For now only one command can be queued at a time.
    if( queue->_first_pending_cmd )
    {
        cmd->_status = MIP_STATUS_CANCELLED;
        return;
    }

    MIP_DIAG_INC(queue->_diag_cmds_queued, 1);

    cmd->_status = MIP_STATUS_PENDING;
    queue->_first_pending_cmd = cmd;
}

void mip_cmd_queue_dequeue(mip_cmd_queue* queue, mip_pending_cmd* cmd)
{
    if( queue->_first_pending_cmd == cmd )
    {
        queue->_first_pending_cmd = NULL;
        cmd->_status = MIP_STATUS_CANCELLED;
    }
}

static enum mip_cmd_result process_fields_for_pending_cmd(mip_pending_cmd* pending, const mip_packet* packet, timeout_type base_timeout, timestamp_type timestamp)
{
    assert( pending->_status != MIP_STATUS_NONE );         // pending->_status must be set to MIP_STATUS_PENDING in mip_cmd_queue_enqueue to get here.
    assert( !mip_cmd_result_is_finished(pending->_status) );  // Command shouldn't be finished yet - make sure the queue is processed properly.

    if( pending->_status == MIP_STATUS_PENDING )
    {
        // Update the timeout to the timestamp of the timeout time.
        pending->_timeout_time = timestamp + base_timeout + pending->_extra_timeout;
        pending->_status = MIP_STATUS_WAITING;
    }

    // ------+------+------+------+------+------+------+------+------+------------------------
    //  ...  | 0x02 | 0xF1 | cmd1 | nack | 0x02 | 0xF1 | cmd2 |  ack |  response field ...
    // ------+------+------+------+------+------+------+------+------+------------------------

    if( mip_packet_descriptor_set(packet) == pending->_descriptor_set )
    {
        mip_field field = {0};
        while( mip_field_next_in_packet(&field, packet) )
        {
            // Not an ack/nack reply field, skip it.
            if( mip_field_field_descriptor(&field) != MIP_REPLY_DESC_GLOBAL_ACK_NACK )
                continue;

            // Sanity check payload length before accessing it.
            if( mip_field_payload_length(&field) != 2 )
                continue;

            const uint8_t* const payload = mip_field_payload(&field);

            const uint8_t cmd_descriptor = payload[MIP_INDEX_REPLY_DESCRIPTOR];
            const uint8_t ack_code       = payload[MIP_INDEX_REPLY_ACK_CODE];

            // Is this the right command reply?
            if( pending->_field_descriptor != cmd_descriptor )
                continue;

            // Descriptor matches!

            uint8_t response_length = 0;
            mip_field response_field;

            // If the command was ACK'd, check if response data is expected.
            if( pending->_response_descriptor != 0x00 && ack_code == MIP_ACK_OK )
            {
                // Look ahead one field for response data.
                response_field = mip_field_next_after(&field);
                if( mip_field_is_valid(&response_field) )
                {
                    const uint8_t response_descriptor = mip_field_field_descriptor(&response_field);

                    // This is a wildcard to accept any response data descriptor.
                    // Needed when the response descriptor is not known or is wrong.
                    if( pending->_response_descriptor == MIP_REPLY_DESC_GLOBAL_ACK_NACK )
                        pending->_response_descriptor = response_descriptor;

                    // Make sure the response descriptor matches what is expected.
                    if( response_descriptor == pending->_response_descriptor )
                    {
                        // Update the response_size field to reflect the actual size.
                        response_length = mip_field_payload_length(&response_field);

                        // Skip this field when iterating for next ack/nack reply.
                        field = response_field;
                    }
                }
            }

            // Limit response data size to lesser of buffer size or actual response length.
            pending->_response_length = (response_length < pending->_response_buffer_size) ? response_length : pending->_response_buffer_size;

            // Copy response data to the pending buffer (skip if response_field is invalid).
            if( pending->_response_length > 0 )
                memcpy(pending->_response_buffer, mip_field_payload(&response_field), pending->_response_length);

            // pending->_ack_code   = ack_code;
            pending->_reply_time = timestamp;  // Completion time

            return (enum mip_cmd_result)ack_code;
        }
    }

    // No matching reply descriptors in this packet.

    // Check for timeout
    if( mip_pending_cmd_check_timeout(pending, timestamp) )
    {
        pending->_response_length = 0;
        // pending->_ack_code        = MIP_NACK_COMMAND_TIMEOUT;

        // Must be last!
        return MIP_STATUS_TIMEDOUT;
    }

    return pending->_status;
}

void mip_cmd_queue_process_packet(mip_cmd_queue* queue, const mip_packet* packet, timestamp_type timestamp)
{
    // Check if the packet is a command descriptor set.
    const uint8_t descriptor_set = mip_packet_descriptor_set(packet);
    if( descriptor_set >= 0x80 && descriptor_set < 0xF0 )
        return;

    if( queue->_first_pending_cmd )
    {
        mip_pending_cmd* pending = queue->_first_pending_cmd;

        const enum mip_cmd_result status = process_fields_for_pending_cmd(pending, packet, queue->_base_timeout, timestamp);

        if( mip_cmd_result_is_finished(status) )
        {
            queue->_first_pending_cmd = queue->_first_pending_cmd->_next;

            // This must be done last b/c it may trigger the thread which queued the command.
            // The command could go out of scope or its attributes inspected.
            pending->_status = status;

#ifdef MIP_ENABLE_DIAGNOSTICS
            if( mip_cmd_result_is_ack(status) )
                MIP_DIAG_INC(queue->_diag_cmds_acked, 1);
            else if( mip_cmd_result_is_reply(status) )
                MIP_DIAG_INC(queue->_diag_cmds_nacked, 1);
            else if( status == MIP_STATUS_TIMEDOUT )
                MIP_DIAG_INC(queue->_diag_cmds_timedout, 1);
            else
                MIP_DIAG_INC(queue->_diag_cmds_failed, 1);
#endif // MIP_ENABLE_DIAGNOSTICS
        }
    }
}

void mip_cmd_queue_clear(mip_cmd_queue* queue)
{
    while( queue->_first_pending_cmd )
    {
        mip_pending_cmd* pending = queue->_first_pending_cmd;
        queue->_first_pending_cmd = pending->_next;

        // This may deallocate the pending command in another thread (make sure to fetch the next cmd first).
        pending->_status = MIP_STATUS_CANCELLED;
    }
}

void mip_cmd_queue_update(mip_cmd_queue* queue, timestamp_type now)
{
    if( queue->_first_pending_cmd )
    {
        mip_pending_cmd* pending = queue->_first_pending_cmd;

        if( pending->_status == MIP_STATUS_PENDING )
        {
            // Update the timeout to the timestamp of the timeout time.
            pending->_timeout_time = now + queue->_base_timeout + pending->_extra_timeout;
            pending->_status = MIP_STATUS_WAITING;
        }
        else if( mip_pending_cmd_check_timeout(pending, now) )
        {
            queue->_first_pending_cmd = queue->_first_pending_cmd->_next;

            // Clear response length and mark when it timed out.
            pending->_response_length = 0;
            pending->_reply_time = now;

            // This must be last!
            pending->_status = MIP_STATUS_TIMEDOUT;

            MIP_DIAG_INC(queue->_diag_cmds_timedout, 1);
        }
    }
}

void mip_cmd_queue_set_base_reply_timeout(mip_cmd_queue* queue, timeout_type timeout)
{
    queue->_base_timeout = timeout;
}

timeout_type mip_cmd_queue_base_reply_timeout(const mip_cmd_queue* queue)
{
    return queue->_base_timeout;
}


#ifdef MIP_ENABLE_DIAGNOSTICS

uint16_t mip_cmd_queue_diagnostic_cmds_queued(const mip_cmd_queue* queue)
{
    return queue->_diag_cmds_queued;
}

uint16_t mip_cmd_queue_diagnostic_cmds_failed(const mip_cmd_queue* queue)
{
    return (uint16_t)queue->_diag_cmds_nacked + queue->_diag_cmds_failed + queue->_diag_cmds_timedout;
}

uint16_t mip_cmd_queue_diagnostic_cmds_successful(const mip_cmd_queue* queue)
{
    return queue->_diag_cmds_acked;
}

uint16_t mip_cmd_queue_diagnostic_cmd_acks(const mip_cmd_queue* queue)
{
    return queue->_diag_cmds_acked;
}

uint16_t mip_cmd_queue_diagnostic_cmd_nacks(const mip_cmd_queue* queue)
{
    return queue->_diag_cmds_nacked;
}

uint16_t mip_cmd_queue_diagnostic_cmd_timeouts(const mip_cmd_queue* queue)
{
    return queue->_diag_cmds_timedout;
}

uint16_t mip_cmd_queue_diagnostic_cmd_errors(const mip_cmd_queue* queue)
{
    return queue->_diag_cmds_failed;
}

#endif // MIP_ENABLE_DIAGNOSTICS