extra_monitor_support.c

Go to the documentation of this file.

/*
 * Copyright (c) 2017 The University of Manchester
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     https://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
 
 
// SARK-based program
#include <sark.h>
#include <stdbool.h>
#include <common-typedefs.h>
#include <spinn_extra.h>
#include "common.h"
#include "data_specification.h"
#include <wfi.h>
 
// Debugging control
//#define DEBUG_DATA_IN
#undef DEBUG_DATA_IN
 
// ------------------------------------------------------------------------
// constants
// ------------------------------------------------------------------------
 
//-----------------------------------------------------------------------------
// stuff to do with SARK DMA
//-----------------------------------------------------------------------------
 
#define DMA_BURST_SIZE 4
 
#define N_DMA_BUFFERS 2
 
//-----------------------------------------------------------------------------
// magic numbers for data speed up extractor
//-----------------------------------------------------------------------------
 
#define END_FLAG_SIZE 4
#define END_FLAG      0xFFFFFFFF
 
enum {
    SEQUENCE_NUMBER_SIZE = 1,
    TRANSACTION_ID_SIZE = 1,
    SDP_PAYLOAD_WORDS =
            ITEMS_PER_DATA_PACKET - SEQUENCE_NUMBER_SIZE - TRANSACTION_ID_SIZE,
    SDP_PAYLOAD_BYTES = SDP_PAYLOAD_WORDS * sizeof(uint)
};
 
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
 
typedef enum data_out_sdp_commands {
    SDP_CMD_START_SENDING_DATA = 100,
    SDP_CMD_START_OF_MISSING_SDP_PACKETS = 1000,
    SDP_CMD_MORE_MISSING_SDP_PACKETS = 1001,
    SDP_CMD_CLEAR = 2000
} data_out_sdp_commands;
 
#define SDP_TIMEOUT 1000
 
#define LENGTH_OF_SDP_HEADER 8
 
//-----------------------------------------------------------------------------
// speed up Data in stuff
//-----------------------------------------------------------------------------
 
#define N_ROUTER_ENTRIES           1024
 
#define INVALID_ROUTER_ENTRY_KEY   0xFFFFFFFF
 
#define INVALID_ROUTER_ENTRY_MASK  0x00000000
 
#define INVALID_ROUTER_ENTRY_ROUTE 0xFF000000
 
#define APP_ID_MASK_FROM_FREE      0x000000FF
 
#define APP_ID_OFFSET_FROM_FREE    24
 
#define N_BASIC_SYSTEM_ROUTER_ENTRIES 1
 
#define N_USABLE_ROUTER_ENTRIES    (N_ROUTER_ENTRIES - N_BASIC_SYSTEM_ROUTER_ENTRIES)
 
//-----------------------------------------------------------------------------
// reinjection functionality magic numbers
//-----------------------------------------------------------------------------
 
#define TDMA_WAIT_PERIOD   0
 
#define ROUTER_INITIAL_TIMEOUT 0x4f
 
#define TICK_PERIOD        10
 
#define PKT_QUEUE_SIZE     4096
 
#define ROUTER_TIMEOUT_MAX 0xFF
 
//-----------------------------------------------------------------------------
// VIC slots assigned
//-----------------------------------------------------------------------------
 
enum {
    CPU_SLOT = SLOT_0,
    CC_SLOT = SLOT_1,
    TIMER_SLOT = SLOT_2,
    DMA_SLOT = SLOT_3,
    DMA_ERROR_SLOT = SLOT_4,
    DMA_TIMEOUT_SLOT = SLOT_5,
    MC_PAYLOAD_SLOT = SLOT_6
};
 
// ------------------------------------------------------------------------
// structs used in system
// ------------------------------------------------------------------------
 
typedef struct dumped_packet_t {
    router_packet_header_t hdr; 
    uint key;                   
    uint pld;                   
} dumped_packet_t;
 
typedef struct pkt_queue_t {
    uint head; 
    uint tail; 
    dumped_packet_t queue[PKT_QUEUE_SIZE];
} pkt_queue_t;
 
enum dma_tags_for_data_speed_up {
    DMA_TAG_READ_FOR_TRANSMISSION = 0,
    DMA_TAG_READ_FOR_RETRANSMISSION = 1,
    DMA_TAG_RETRANSMISSION_READING = 2,
    DMA_TAG_FOR_WRITING_MISSING_SEQ_NUMS = 3
};
 
typedef struct sdp_data_out_t {
    data_out_sdp_commands command;
    uint transaction_id;
    address_t sdram_location;
    uint length;
} sdp_data_out_t;
 
typedef struct router_entry_t {
    uint32_t key;   
    uint32_t mask;  
    uint32_t route; 
} router_entry_t;
 
typedef struct data_in_data_items {
    uint32_t address_mc_key;
    uint32_t data_mc_key;
    uint32_t boundary_mc_key;
    uint32_t n_system_router_entries;
    router_entry_t system_router_entries[];
} data_in_data_items_t;
 
enum missing_seq_num_data_positions {
    POSITION_OF_NO_MISSING_SEQ_PACKETS = 2,
    START_OF_MISSING_MORE = 2,
    START_OF_MISSING_SEQ_NUMS = 3,
};
 
typedef struct reinjector_status_response_packet_t {
    uint router_timeout;
    uint router_emergency_timeout;
    uint n_dropped_packets;
    uint n_missed_dropped_packets;
    uint n_dropped_packets_overflows;
    uint n_reinjected_packets;
    uint n_link_dumped_packets;
    uint n_processor_dumped_packets;
    uint packet_types_reinjected;
    uint link_proc_bits;
} reinjector_status_response_packet_t;
 
typedef struct reinject_config_t {
    uint multicast_flag;
    uint point_to_point_flag;
    uint fixed_route_flag;
    uint nearest_neighbour_flag;
    uint reinjection_base_mc_key;
} reinject_config_t;
 
enum functionality_to_port_num_map {
    REINJECTION_PORT = 4,
    DATA_SPEED_UP_OUT_PORT = 5,
    DATA_SPEED_UP_IN_PORT = 6
};
 
enum data_spec_regions {
    CONFIG_REINJECTION = 0,
    CONFIG_DATA_SPEED_UP_OUT = 1,
    CONFIG_DATA_SPEED_UP_IN = 2,
    PROVENANCE_REGION = 3
};
 
enum speed_up_in_command {
    SDP_COMMAND_FOR_SAVING_APPLICATION_MC_ROUTING = 6,
    SDP_COMMAND_FOR_LOADING_APPLICATION_MC_ROUTES = 7,
    SDP_COMMAND_FOR_LOADING_SYSTEM_MC_ROUTES = 8
};
 
typedef struct data_speed_out_config_t {
    uint my_key;
    uint new_seq_key;
    uint first_data_key;
    uint transaction_id_key;
    uint end_flag_key;
} data_speed_out_config_t;
 
typedef struct extra_monitor_provenance_t {
    uint n_sdp_packets;
    uint n_in_streams;
    uint n_out_streams;
    uint n_router_changes;
} extra_monitor_provenance_t;
 
enum callback_priorities {
    SDP = 0,
    DMA = 0
};
 
// ------------------------------------------------------------------------
// global variables for reinjector functionality
// ------------------------------------------------------------------------
 
static uint reinject_p2p_source_id;
 
static pkt_queue_t reinject_pkt_queue;
 
// statistics
static uint reinject_n_dropped_packets;
 
static uint reinject_n_missed_dropped_packets;
 
static uint reinject_n_dropped_packet_overflows;
 
static uint reinject_n_reinjected_packets;
 
static uint reinject_n_link_dumped_packets;
 
static uint reinject_n_processor_dumped_packets;
 
static uint reinject_link_proc_bits;
 
// Determine what to reinject
 
static bool reinject_mc;
 
static bool reinject_pp;
 
static bool reinject_nn;
 
static bool reinject_fr;
 
static bool reinject_run = true;
 
// ------------------------------------------------------------------------
// global variables for data speed up in functionality
// ------------------------------------------------------------------------
 
// data in variables
static router_entry_t *data_in_saved_application_router_table = NULL;
 
static uint data_in_address_key = 0;
 
static uint data_in_data_key = 0;
 
static uint data_in_boundary_key = 0;
 
static address_t data_in_write_address = NULL;
 
static address_t data_in_first_write_address = NULL;
 
static int data_in_application_table_n_valid_entries = 0;
 
static bool data_in_last_table_load_was_system = false;
 
// ------------------------------------------------------------------------
// global variables for data speed up out functionality
// ------------------------------------------------------------------------
 
// transmission stuff
 
static uint32_t data_out_data_to_transmit[N_DMA_BUFFERS][ITEMS_PER_DATA_PACKET];
 
static uint32_t data_out_transmit_dma_pointer = 0;
 
static uint32_t data_out_position_in_store = 0;
 
static uint32_t data_out_num_items_read = 0;
 
static uint32_t data_out_transaction_id = 0;
 
static bool data_out_first_transmission = true;
 
static bool data_out_has_finished = false;
 
static uint32_t data_out_retransmitted_seq_num_items_read = 0;
 
// retransmission stuff
 
static uint32_t data_out_n_missing_seq_packets = 0;
 
static uint32_t data_out_n_missing_seq_nums_in_sdram = 0;
 
static uint32_t data_out_n_elements_to_read_from_sdram = 0;
 
static address_t data_out_missing_seq_num_sdram_address = NULL;
 
static uint32_t data_out_max_seq_num = 0;
 
// retransmission DMA stuff
 
static uint32_t data_out_retransmit_seq_nums[ITEMS_PER_DATA_PACKET];
 
static uint32_t data_out_position_for_retransmission = 0;
 
static uint32_t data_out_missing_seq_num_being_processed = 0;
 
static uint32_t data_out_read_data_position = 0;
 
static uint32_t data_out_dma_port_last_used = 0;
 
static bool data_out_in_retransmission_mode = false;
 
static address_t data_out_store_address = NULL;
 
static uint32_t data_out_basic_data_key = 0;
 
static uint32_t data_out_new_sequence_key = 0;
 
static uint32_t data_out_first_data_key = 0;
 
static uint32_t data_out_transaction_id_key = 0;
 
static uint32_t data_out_end_flag_key = 0;
 
static bool data_out_stop = false;
 
// ------------------------------------------------------------------------
// support functions and variables
// ------------------------------------------------------------------------
 
extern INT_HANDLER sark_int_han(void);
 
static ushort my_addr;
 
static vcpu_t *const _sark_virtual_processor_info = (vcpu_t *) SV_VCPU;
 
static extra_monitor_provenance_t *prov;
 
static data_specification_metadata_t *dse_regions;
 
static inline void *dse_block(uint index) {
    return data_specification_get_region(index, dse_regions);
}
 
static void publish_transaction_id(int transaction_id) {
    _sark_virtual_processor_info[sark.virt_cpu].user1 = transaction_id;
}
 
static inline void *sdram_alloc(uint size) {
    return sark_xalloc(sv->sdram_heap, size, 0,
            ALLOC_LOCK | ALLOC_ID | (sark_vec->app_id << 8));
}
 
static inline void sdram_free(void *data) {
    sark_xfree(sv->sdram_heap, data,
            ALLOC_LOCK | ALLOC_ID | (sark_vec->app_id << 8));
}
 
static inline uint sdram_max_block_size(void) {
    return sark_heap_max(sv->sdram_heap, ALLOC_LOCK);
}
 
static inline sdp_msg_t *get_message_from_mailbox(void) {
    sdp_msg_t *shm_msg = (sdp_msg_t *) sark.vcpu->mbox_ap_msg;
    sdp_msg_t *msg = sark_msg_get();
    if (msg != NULL) {
        sark_msg_cpy(msg, shm_msg);
    }
    sark_shmsg_free(shm_msg);
    sark.vcpu->mbox_ap_cmd = SHM_IDLE;
    return msg;
}
 
static inline void vic_interrupt_done(void) {
    vic_control->vector_address = (vic_interrupt_handler_t) vic;
}
 
static inline void set_vic_callback(
        uint8_t slot, uint type, vic_interrupt_handler_t callback) {
    vic_interrupt_vector[slot] = callback;
    vic_interrupt_control[slot] = (vic_vector_control_t) {
        .source = type,
        .enable = true
    };
}
 
// ------------------------------------------------------------------------
// reinjector main functions
// ------------------------------------------------------------------------
 
static inline void reinjection_enable_comms_interrupt(void) {
    vic_control->int_enable = (vic_mask_t) {
        .cc_tx_not_full = true
    };
}
 
static inline void reinjection_disable_comms_interrupt(void) {
    vic_control->int_disable = (vic_mask_t) {
        .cc_tx_not_full = true
    };
}
 
static inline bool reinjection_can_send_now(void) {
    return router_control->status.output_stage == ROUTER_OUTPUT_STAGE_EMPTY;
}
 
static INT_HANDLER reinjection_timer_callback(void) {
    // clear interrupt in timer,
    timer1_control->interrupt_clear = true;
 
    // check if router not blocked
    if (reinjection_can_send_now()) {
        // access packet queue with FIQ disabled,
        uint cpsr = cpu_fiq_disable();
 
        // if queue not empty turn on packet bouncing,
        if (reinject_pkt_queue.tail != reinject_pkt_queue.head) {
            // restore FIQ after queue access,
            cpu_int_restore(cpsr);
 
            // enable communications controller. interrupt to bounce packets
            reinjection_enable_comms_interrupt();
        } else {
            // restore FIQ after queue access
            cpu_int_restore(cpsr);
        }
    }
 
    // and tell VIC we're done
    vic_interrupt_done();
}
 
static inline void reinjection_reinject_packet(const dumped_packet_t *pkt) {
    // write header and route
    comms_control->tx_control = (comms_tx_control_t) {
        .control_byte = pkt->hdr.control
    };
    comms_control->source_addr = (comms_source_addr_t) {
        .p2p_source_id = reinject_p2p_source_id,
        .route = pkt->hdr.route
    };
 
    // maybe write payload,
    spinnaker_packet_control_byte_t control =
            (spinnaker_packet_control_byte_t) pkt->hdr.control;
    if (control.payload) {
        comms_control->tx_data = pkt->pld;
    }
 
    // write key to fire packet,
    comms_control->tx_key = pkt->key;
 
    // Add to statistics
    reinject_n_reinjected_packets++;
}
 
static INT_HANDLER reinjection_ready_to_send_callback(void) {
    // TODO: may need to deal with packet timestamp.
 
    // check if router not blocked
    if (reinjection_can_send_now()) {
        // access packet queue with FIQ disabled,
        uint cpsr = cpu_fiq_disable();
 
        // if queue not empty bounce packet,
        if (reinject_pkt_queue.tail != reinject_pkt_queue.head) {
            // dequeue packet and update queue pointer
            dumped_packet_t pkt =
                    reinject_pkt_queue.queue[reinject_pkt_queue.head];
            reinject_pkt_queue.head =
                    (reinject_pkt_queue.head + 1) % PKT_QUEUE_SIZE;
 
            // restore FIQ queue access,
            cpu_int_restore(cpsr);
 
            // reinject the packet
            reinjection_reinject_packet(&pkt);
        } else {
            // restore FIQ after queue access,
            cpu_int_restore(cpsr);
 
            // and disable communications controller interrupts; queue empty!
            reinjection_disable_comms_interrupt();
        }
    } else {
        // disable communications controller interrupts
        reinjection_disable_comms_interrupt();
    }
 
    // and tell VIC we're done
    vic_interrupt_done();
}
 
static inline void extract_dropped_packet_details(
        router_dump_outputs_t rtr_dump_outputs) {
    if (rtr_dump_outputs.processor > 0) {
        // add to the count the number of active bits from this dumped
        // packet, as this indicates how many processors this packet
        // was meant to go to.
        reinject_n_processor_dumped_packets +=
                __builtin_popcount(rtr_dump_outputs.processor);
        reinject_link_proc_bits |= rtr_dump_outputs.processor << 6;
    }
 
    if (rtr_dump_outputs.link > 0) {
        // add to the count the number of active bits from this dumped
        // packet, as this indicates how many links this packet was
        // meant to go to.
        reinject_n_link_dumped_packets +=
                __builtin_popcount(rtr_dump_outputs.link);
        reinject_link_proc_bits |= rtr_dump_outputs.link & 0x3F;
    }
}
 
static INT_HANDLER reinjection_dropped_packet_callback(void) {
    // get packet from router,
    router_packet_header_t hdr = router_control->dump.header;
    uint pld = router_control->dump.payload;
    uint key = router_control->dump.key;
 
    // clear dump status and interrupt in router,
    router_dump_outputs_t rtr_dump_outputs = router_control->dump.outputs;
    router_dump_status_t rtr_dstat = router_control->dump.status;
 
    // only reinject if configured
    uint packet_type = ((spinnaker_packet_control_byte_t) hdr.control).type;
    if (((packet_type == SPINNAKER_PACKET_TYPE_MC) && reinject_mc) ||
            ((packet_type == SPINNAKER_PACKET_TYPE_P2P) && reinject_pp) ||
            ((packet_type == SPINNAKER_PACKET_TYPE_NN) && reinject_nn) ||
            ((packet_type == SPINNAKER_PACKET_TYPE_FR) && reinject_fr)) {
 
        // check for overflow from router
        if (rtr_dstat.overflow) {
            reinject_n_missed_dropped_packets++;
        } else {
            // Note that the processor_dump and link_dump flags are sticky
            // so you can only really count these if you *haven't* missed a
            // dropped packet - hence this being split out
            extract_dropped_packet_details(rtr_dump_outputs);
        }
 
        // Only update this counter if this is a packet to reinject
        reinject_n_dropped_packets++;
 
        // Disable FIQ for queue access
        uint cpsr = cpu_fiq_disable();
 
        // try to insert dumped packet in the queue,
        uint new_tail = (reinject_pkt_queue.tail + 1) % PKT_QUEUE_SIZE;
 
        // check for space in the queue
        if (new_tail != reinject_pkt_queue.head) {
            // queue packet,
            reinject_pkt_queue.queue[reinject_pkt_queue.tail].hdr = hdr;
            reinject_pkt_queue.queue[reinject_pkt_queue.tail].key = key;
            reinject_pkt_queue.queue[reinject_pkt_queue.tail].pld = pld;
 
            // update queue pointer,
            reinject_pkt_queue.tail = new_tail;
        } else {
            // The queue of packets has overflowed
            reinject_n_dropped_packet_overflows++;
        }
 
        // restore FIQ after queue access,
        cpu_int_restore(cpsr);
    } else {
        // Even if not re-injecting, we can still get this level of detail
        extract_dropped_packet_details(rtr_dump_outputs);
    }
}
 
static void reinjection_read_packet_types(const reinject_config_t *config) {
    // process multicast reinject flag
    if (config->multicast_flag == 1) {
        reinject_mc = false;
    } else {
        reinject_mc = true;
    }
 
    // process point to point flag
    if (config->point_to_point_flag == 1) {
        reinject_pp = false;
    } else {
        reinject_pp = true;
    }
 
    // process fixed route flag
    if (config->fixed_route_flag == 1) {
        reinject_fr = false;
    } else {
        reinject_fr = true;
    }
 
    // process fixed route flag
    if (config->nearest_neighbour_flag == 1) {
        reinject_nn = false;
    } else {
        reinject_nn = true;
    }
 
    io_printf(IO_BUF,
            "[INFO] Setting reinject mc to %d\n[INFO] Setting reinject pp to %d\n"
            "[INFO] Setting reinject fr to %d\n[INFO] Setting reinject nn to %d\n",
            reinject_mc, reinject_pp, reinject_fr, reinject_nn);
 
    // set the reinjection mc api
    initialise_reinjection_mc_api(config->reinjection_base_mc_key);
 
}
 
static inline void reinjection_set_wait1_timeout(uint payload) {
    router_control->control.begin_emergency_wait_time = payload;
    prov->n_router_changes++;
}
 
static inline void reinjection_set_wait2_timeout(uint payload) {
    router_control->control.drop_wait_time = payload;
    prov->n_router_changes++;
}
 
static inline int reinjection_set_timeout_sdp(sdp_msg_t *msg) {
#ifdef DEBUG_REINJECTOR
    io_printf(IO_BUF, "[DEBUG] setting router timeouts via sdp\n");
#endif
    if (msg->arg1 > ROUTER_TIMEOUT_MAX) {
        msg->cmd_rc = RC_ARG;
        return 0;
    }
 
    router_control->control.begin_emergency_wait_time = msg->arg1;
    prov->n_router_changes++;
 
    // set SCP command to OK, as successfully completed
    msg->cmd_rc = RC_OK;
    return 0;
}
 
static inline int reinjection_set_emergency_timeout_sdp(sdp_msg_t *msg) {
#ifdef DEBUG_REINJECTOR
    io_printf(IO_BUF, "[DEBUG] setting router emergency timeouts via sdp\n");
#endif
    if (msg->arg1 > ROUTER_TIMEOUT_MAX) {
        msg->cmd_rc = RC_ARG;
        return 0;
    }
 
    router_control->control.drop_wait_time = msg->arg1;
    prov->n_router_changes++;
 
    // set SCP command to OK, as successfully completed
    msg->cmd_rc = RC_OK;
    return 0;
}
 
static inline int reinjection_set_packet_types(sdp_msg_t *msg) {
    reinject_mc = msg->arg1;
    reinject_pp = msg->arg2;
    reinject_fr = msg->arg3;
    reinject_nn = msg->data[0];
    prov->n_router_changes++;
 
    io_printf(IO_BUF,
            "[INFO] Setting reinject mc to %d\n[INFO] Setting reinject pp to %d\n"
            "[INFO] Setting reinject fr to %d\n[INFO] Setting reinject nn to %d\n",
            reinject_mc, reinject_pp, reinject_fr, reinject_nn);
 
    // set SCP command to OK, as successfully completed
    msg->cmd_rc = RC_OK;
    return 0;
}
 
static inline int reinjection_get_status(sdp_msg_t *msg) {
    reinjector_status_response_packet_t *data =
            (reinjector_status_response_packet_t *) &msg->arg1;
 
    // Put the router timeouts in the packet
    router_control_t control = router_control->control;
    data->router_timeout = control.begin_emergency_wait_time;
    data->router_emergency_timeout = control.drop_wait_time;
 
    // Put the statistics in the packet
    data->n_dropped_packets = reinject_n_dropped_packets;
    data->n_missed_dropped_packets = reinject_n_missed_dropped_packets;
    data->n_dropped_packets_overflows = reinject_n_dropped_packet_overflows;
    data->n_reinjected_packets = reinject_n_reinjected_packets;
    data->n_link_dumped_packets = reinject_n_link_dumped_packets;
    data->n_processor_dumped_packets = reinject_n_processor_dumped_packets;
    data->link_proc_bits = reinject_link_proc_bits;
 
    // Put the current services enabled in the packet
    data->packet_types_reinjected = 0;
    bool values_to_check[] = {reinject_mc, reinject_pp,
                              reinject_nn, reinject_fr};
    for (int i = 0; i < 4; i++) {
        data->packet_types_reinjected |= (values_to_check[i] << i);
    }
 
    // set SCP command to OK, as successfully completed
    msg->cmd_rc = RC_OK;
    // Return the number of bytes in the packet
    return sizeof(reinjector_status_response_packet_t);
}
 
static inline int reinjection_reset_counters(sdp_msg_t *msg) {
    reinject_n_dropped_packets = 0;
    reinject_n_missed_dropped_packets = 0;
    reinject_n_dropped_packet_overflows = 0;
    reinject_n_reinjected_packets = 0;
    reinject_n_link_dumped_packets = 0;
    reinject_n_processor_dumped_packets = 0;
    reinject_link_proc_bits = 0;
 
    // set SCP command to OK, as successfully completed
    msg->cmd_rc = RC_OK;
    return 0;
}
 
static inline int reinjection_exit(sdp_msg_t *msg) {
    vic_control->int_disable = (vic_mask_t) {
        .timer1 = true,
        .router_dump = true
    };
    reinjection_disable_comms_interrupt();
    vic_control->int_select = (vic_mask_t) {
        // Also all the rest are not FIQ
        .router_dump = false
    };
    reinject_run = false;
 
    // set SCP command to OK, as successfully completed
    msg->cmd_rc = RC_OK;
    return 0;
}
 
static void reinjection_clear(void) {
    // Disable FIQ for queue access
    uint cpsr = cpu_fiq_disable();
    // Clear any stored dropped packets
    reinject_pkt_queue.head = 0;
    reinject_pkt_queue.tail = 0;
    // restore FIQ after queue access,
    cpu_int_restore(cpsr);
    // and disable communications controller interrupts
    reinjection_disable_comms_interrupt();
}
 
static inline int reinjection_clear_message(sdp_msg_t *msg) {
    reinjection_clear();
    prov->n_router_changes++;
    // set SCP command to OK, as successfully completed
    msg->cmd_rc = RC_OK;
    return 0;
}
 
static uint reinjection_sdp_command(sdp_msg_t *msg) {
    switch (msg->cmd_rc) {
#ifdef DEBUG_REINJECTOR
    io_printf(IO_BUF, "[DEBUG] seq %d\n", msg->seq);
#endif
    case CMD_DPRI_SET_ROUTER_TIMEOUT:
#ifdef DEBUG_REINJECTOR
        io_printf(IO_BUF, "[DEBUG] router timeout\n");
#endif
        return reinjection_set_timeout_sdp(msg);
    case CMD_DPRI_SET_ROUTER_EMERGENCY_TIMEOUT:
#ifdef DEBUG_REINJECTOR
        io_printf(IO_BUF, "[DEBUG] router emergency timeout\n");
#endif
        return reinjection_set_emergency_timeout_sdp(msg);
    case CMD_DPRI_SET_PACKET_TYPES:
#ifdef DEBUG_REINJECTOR
        io_printf(IO_BUF, "[DEBUG] router set packet type\n");
#endif
        return reinjection_set_packet_types(msg);
    case CMD_DPRI_GET_STATUS:
#ifdef DEBUG_REINJECTOR
        io_printf(IO_BUF, "[DEBUG] router get status\n");
#endif
        return reinjection_get_status(msg);
    case CMD_DPRI_RESET_COUNTERS:
#ifdef DEBUG_REINJECTOR
        io_printf(IO_BUF, "[DEBUG] router reset\n");
#endif
        return reinjection_reset_counters(msg);
    case CMD_DPRI_EXIT:
#ifdef DEBUG_REINJECTOR
        io_printf(IO_BUF, "[DEBUG] router exit\n");
#endif
        return reinjection_exit(msg);
    case CMD_DPRI_CLEAR:
#ifdef DEBUG_REINJECTOR
        io_printf(IO_BUF, "[DEBUG] router clear\n");
#endif
        return reinjection_clear_message(msg);
    default:
        // If we are here, the command was not recognised, so fail (ARG as the
        // command is an argument)
        msg->cmd_rc = RC_ARG;
        return 0;
    }
}
 
static void reinjection_configure_timer(void) {
    // Clear the interrupt
    timer1_control->control = (timer_control_t) {
        .enable = false,
        .interrupt_enable = false
    };
    timer1_control->interrupt_clear = true;
 
    // Set the timer times
    timer1_control->load_value = sv->cpu_clk * TICK_PERIOD;
    timer1_control->background_load_value = sv->cpu_clk * TICK_PERIOD;
}
 
static void reinjection_configure_comms_controller(void) {
    // remember SAR register contents (p2p source ID)
    reinject_p2p_source_id = comms_control->source_addr.p2p_source_id;
}
 
static void reinjection_configure_router(void) {
    // re-configure wait values in router
    router_control_t control = router_control->control;
    control.begin_emergency_wait_time = ROUTER_INITIAL_TIMEOUT;
    control.drop_wait_time = 0;
    router_control->control = control;
 
    // clear router interrupts,
    (void) router_control->status;
 
    // clear router dump status,
    (void) router_control->dump.status;
 
    // clear router error status,
    (void) router_control->error.status;
 
    // and enable router interrupts when dumping packets, and count errors
    control.dump_interrupt_enable = true;
    control.count_framing_errors = true;
    control.count_parity_errors = true;
    control.count_timestamp_errors = true;
    router_control->control = control;
}
 
//-----------------------------------------------------------------------------
// data in speed up main functions
//-----------------------------------------------------------------------------
 
static void data_in_clear_router(void) {
    rtr_entry_t router_entry;
 
    // clear the currently loaded routing table entries
    for (uint entry_id = N_BASIC_SYSTEM_ROUTER_ENTRIES;
            entry_id < N_ROUTER_ENTRIES; entry_id++) {
        if (rtr_mc_get(entry_id, &router_entry) &&
                router_entry.key != INVALID_ROUTER_ENTRY_KEY &&
                router_entry.mask != INVALID_ROUTER_ENTRY_MASK) {
            rtr_free(entry_id, 1);
        }
    }
#ifdef DEBUG_DATA_IN
    io_printf(IO_BUF, "[DEBUG] max free block is %d\n", rtr_alloc_max());
#endif
}
 
static inline void data_in_process_boundary(void) {
    if (data_in_write_address) {
#ifdef DEBUG_DATA_IN
        io_printf(IO_BUF, "[DEBUG] Wrote %u words\n",
                data_in_write_address - data_in_first_write_address);
#endif
        data_in_write_address = NULL;
    }
    data_in_first_write_address = NULL;
}
 
static inline void data_in_process_address(uint data) {
    if (data_in_write_address) {
        data_in_process_boundary();
    }
#ifdef DEBUG_DATA_IN
    io_printf(IO_BUF, "[DEBUG] Setting write address to 0x%08x\n", data);
#endif
    data_in_first_write_address = data_in_write_address = (address_t) data;
}
 
static inline void data_in_process_data(uint data) {
    // data keys require writing to next point in sdram
 
    if (data_in_write_address == NULL) {
        io_printf(IO_BUF, "[ERROR] Write address not set when write data received!\n");
        rt_error(RTE_SWERR);
    }
    *data_in_write_address = data;
    data_in_write_address++;
}
 
static INT_HANDLER process_mc_payload_packet(void) {
    // get data from comm controller
    uint data = comms_control->rx_data;
    uint key = comms_control->rx_key;
 
    if (key == reinject_timeout_mc_key) {
        reinjection_set_wait1_timeout(data);
    } else if (key == reinject_emergency_timeout_mc_key) {
        reinjection_set_wait2_timeout(data);
    } else if (key == reinject_clear_mc_key) {
        reinjection_clear();
    } else if (key == data_in_address_key) {
        data_in_process_address(data);
    } else if (key == data_in_data_key) {
        data_in_process_data(data);
    } else if (key == data_in_boundary_key) {
        prov->n_in_streams++;
        data_in_process_boundary();
    } else {
        io_printf(IO_BUF,
                "[WARNING] failed to recognise multicast packet key 0x%08x\n",
                key);
    }
 
    // and tell VIC we're done
    vic_interrupt_done();
}
 
static void data_in_load_router(
        router_entry_t *sdram_address, uint n_entries) {
#ifdef DEBUG_DATA_IN
    io_printf(IO_BUF, "[DEBUG] Writing %u router entries\n", n_entries);
#endif
    if (n_entries == 0) {
        return;
    }
    uint start_entry_id = rtr_alloc_id(n_entries, sark_app_id());
    if (start_entry_id == 0) {
        io_printf(IO_BUF,
                "[ERROR] Received error with requesting %u router entries.\n",
                n_entries);
        rt_error(RTE_SWERR);
    }
 
    for (uint idx = 0; idx < n_entries; idx++) {
        // check for invalid entries (possible during alloc and free or
        // just not filled in.
        if (sdram_address[idx].key != INVALID_ROUTER_ENTRY_KEY &&
                sdram_address[idx].mask != INVALID_ROUTER_ENTRY_MASK &&
                sdram_address[idx].route != INVALID_ROUTER_ENTRY_ROUTE) {
#ifdef DEBUG_DATA_IN
            // Produces quite a lot of debugging output when enabled
            io_printf(IO_BUF,
                    "[DEBUG] Setting key %08x, mask %08x, route %08x for entry %u\n",
                    sdram_address[idx].key, sdram_address[idx].mask,
                    sdram_address[idx].route, idx + start_entry_id);
#endif
            // try setting the valid router entry
            if (rtr_mc_set(idx + start_entry_id, sdram_address[idx].key,
                    sdram_address[idx].mask, sdram_address[idx].route) != 1) {
                io_printf(IO_BUF, "[WARNING] failed to write router entry %d, "
                        "with key %08x, mask %08x, route %08x\n",
                        idx + start_entry_id, sdram_address[idx].key,
                        sdram_address[idx].mask, sdram_address[idx].route);
            }
        }
    }
    prov->n_router_changes++;
}
 
static void data_in_save_router(void) {
    rtr_entry_t router_entry;
    data_in_application_table_n_valid_entries = 0;
    for (uint entry_id = N_BASIC_SYSTEM_ROUTER_ENTRIES, i = 0;
            entry_id < N_ROUTER_ENTRIES; entry_id++, i++) {
        (void) rtr_mc_get(entry_id, &router_entry);
 
        if (router_entry.key != INVALID_ROUTER_ENTRY_KEY &&
                router_entry.mask != INVALID_ROUTER_ENTRY_MASK &&
                router_entry.route != INVALID_ROUTER_ENTRY_ROUTE) {
            // move to sdram
            data_in_saved_application_router_table[
                    data_in_application_table_n_valid_entries].key = router_entry.key;
            data_in_saved_application_router_table[
                    data_in_application_table_n_valid_entries].mask = router_entry.mask;
            data_in_saved_application_router_table[
                    data_in_application_table_n_valid_entries].route = router_entry.route;
            data_in_application_table_n_valid_entries++;
        }
    }
}
 
static void data_in_speed_up_load_in_system_tables(
        data_in_data_items_t *items) {
    // read in router table into app store in sdram (in case its changed
    // since last time)
    data_in_save_router();
 
    // clear the currently loaded routing table entries to avoid conflicts
    data_in_clear_router();
 
    // read in and load routing table entries
#ifdef DEBUG_DATA_IN
    io_printf(IO_BUF, "[INFO] Loading system routes\n");
#endif
    data_in_load_router(
            items->system_router_entries, items->n_system_router_entries);
}
 
static void data_in_speed_up_load_in_application_routes(void) {
    // clear the currently loaded routing table entries
    data_in_clear_router();
 
    // load app router entries from sdram
#ifdef DEBUG_DATA_IN
    io_printf(IO_BUF, "[INFO] Loading application routes\n");
#endif
    data_in_load_router(
            data_in_saved_application_router_table,
            data_in_application_table_n_valid_entries);
}
 
static uint data_in_speed_up_command(sdp_msg_t *msg) {
    switch (msg->cmd_rc) {
    case SDP_COMMAND_FOR_SAVING_APPLICATION_MC_ROUTING:
#ifdef DEBUG_DATA_IN
        io_printf(IO_BUF, "[INFO] Saving application router entries from router\n");
#endif
        data_in_save_router();
        msg->cmd_rc = RC_OK;
        break;
    case SDP_COMMAND_FOR_LOADING_APPLICATION_MC_ROUTES:
        data_in_speed_up_load_in_application_routes();
        msg->cmd_rc = RC_OK;
        data_in_last_table_load_was_system = false;
        break;
    case SDP_COMMAND_FOR_LOADING_SYSTEM_MC_ROUTES:
        if (data_in_last_table_load_was_system) {
#ifdef DEBUG_DATA_IN
            io_printf(IO_BUF,
                    "[WARNING] Already loaded system router; ignoring but replying\n");
#endif
            msg->cmd_rc = RC_OK;
            break;
        }
        data_in_speed_up_load_in_system_tables(
                dse_block(CONFIG_DATA_SPEED_UP_IN));
        msg->cmd_rc = RC_OK;
        data_in_last_table_load_was_system = true;
        break;
    default:
        io_printf(IO_BUF,
                "[WARNING] Received unknown SDP packet in data in speed up port"
                " with command id %d\n", msg->cmd_rc);
        msg->cmd_rc = RC_ARG;
    }
    return 0;
}
 
//-----------------------------------------------------------------------------
// data speed up out main functions
//-----------------------------------------------------------------------------
 
static inline void send_fixed_route_packet(uint32_t key, uint32_t data) {
    enum {
        TX_NOT_FULL_MASK = 0x10000000
    };
 
    // If stop, don't send anything
    if (data_out_stop) {
        return;
    }
 
    // Wait for a router slot
    while (!comms_control->tx_control.not_full) {
        // Empty body; CC array is volatile
    }
    const spinnaker_packet_control_byte_t fixed_route_with_payload = {
        .payload = true,
        .type = SPINNAKER_PACKET_TYPE_FR
    };
    comms_control->tx_control = (comms_tx_control_t) {
        .control_byte = fixed_route_with_payload.value
    };
    comms_control->tx_data = data;
    comms_control->tx_key = key;
}
 
static void data_out_send_data_block(
        uint32_t current_dma_pointer, uint32_t n_elements_to_send,
        uint32_t first_packet_key, uint32_t second_packet_key) {
    // send data
    for (uint i = 0; i < n_elements_to_send; i++) {
        uint32_t current_data = data_out_data_to_transmit[current_dma_pointer][i];
 
        send_fixed_route_packet(first_packet_key, current_data);
 
        // update key to transmit with
        if (i == 0) {
            first_packet_key = second_packet_key;
        } else {
            first_packet_key = data_out_basic_data_key;
        }
    }
}
 
static inline void data_out_start_dma_read(
        uint32_t dma_tag, void *source, void *destination, uint n_words) {
    data_out_dma_port_last_used = dma_tag;
    dma_control->sdram_address = source;
    dma_control->tcm_address = destination;
    dma_control->description = (dma_description_t) {
        .width = DMA_TRANSFER_DOUBLE_WORD,
        .burst = DMA_BURST_SIZE,
        .direction = DMA_DIRECTION_READ,
        .length_words = n_words
    };
}
 
static void data_out_read(
        uint32_t dma_tag, uint32_t offset, uint32_t items_to_read) {
    // set off DMA
    data_out_transmit_dma_pointer =
            (data_out_transmit_dma_pointer + 1) % N_DMA_BUFFERS;
 
    address_t data_sdram_position =
            &data_out_store_address[data_out_position_in_store];
 
    // update positions as needed
    data_out_position_in_store += items_to_read;
    data_out_num_items_read = items_to_read;
 
    // set off DMA
    data_out_start_dma_read(dma_tag, data_sdram_position,
            &data_out_data_to_transmit[data_out_transmit_dma_pointer][offset],
            items_to_read);
}
 
static void data_out_send_end_flag(void) {
    send_fixed_route_packet(data_out_end_flag_key, END_FLAG);
}
 
static void data_out_dma_complete_reading_for_original_transmission(void) {
    // set up state
    uint32_t current_dma_pointer = data_out_transmit_dma_pointer;
    uint32_t key_to_transmit = data_out_basic_data_key;
    uint32_t second_key_to_transmit = data_out_basic_data_key;
    uint32_t items_read_this_time = data_out_num_items_read;
 
    // put size in bytes if first send
    if (data_out_first_transmission) {
        //io_printf(IO_BUF, "in first\n");
        data_out_data_to_transmit[current_dma_pointer][0] = data_out_max_seq_num;
        data_out_data_to_transmit[current_dma_pointer][1] = data_out_transaction_id;
        key_to_transmit = data_out_first_data_key;
        second_key_to_transmit = data_out_transaction_id_key;
        data_out_first_transmission = false;
        items_read_this_time += 2;
    }
 
    // stopping procedure
    // if a full packet, read another and try again
    if (data_out_position_in_store < data_out_n_elements_to_read_from_sdram) {
        uint32_t num_items_to_read = SDP_PAYLOAD_WORDS;
        uint32_t next_position_in_store =
                data_out_position_in_store + SDP_PAYLOAD_WORDS;
 
        // if less data needed request less data
        if (next_position_in_store >= data_out_n_elements_to_read_from_sdram) {
            num_items_to_read =
                    data_out_n_elements_to_read_from_sdram -
                    data_out_position_in_store;
        }
 
        // set off another read and transmit DMA'ed one
        data_out_read(DMA_TAG_READ_FOR_TRANSMISSION, 0, num_items_to_read);
        data_out_send_data_block(current_dma_pointer, items_read_this_time,
                key_to_transmit, second_key_to_transmit);
    } else {
        data_out_send_data_block(current_dma_pointer, items_read_this_time,
                key_to_transmit, second_key_to_transmit);
 
        // send end flag.
        data_out_send_end_flag();
 
        data_out_has_finished = true;
        data_out_n_missing_seq_packets = 0;
    }
 
    if (TDMA_WAIT_PERIOD != 0) {
        sark_delay_us(TDMA_WAIT_PERIOD);
    }
}
 
static void data_out_write_missing_seq_nums_into_sdram(
        uint32_t data[], uint length, uint32_t start_offset) {
    for (uint i = start_offset, j = data_out_n_missing_seq_nums_in_sdram;
            i < length; i++, j++) {
        data_out_missing_seq_num_sdram_address[j] = data[i];
#ifdef DEBUG_DATA_OUT
        if (data[i] > data_out_max_seq_num) {
            io_printf(IO_BUF, "[WARNING] Storing bad seq num. %d %d\n",
                    data[i], data_out_max_seq_num);
        }
#endif
    }
    data_out_n_missing_seq_nums_in_sdram += length - start_offset;
}
 
static void data_out_store_missing_seq_nums(
        uint32_t data[], uint length, bool first) {
    uint32_t start_reading_offset = START_OF_MISSING_MORE;
    if (first) {
        data_out_n_missing_seq_packets =
                data[POSITION_OF_NO_MISSING_SEQ_PACKETS];
 
        uint32_t size_of_data =
                (data_out_n_missing_seq_packets * ITEMS_PER_DATA_PACKET * sizeof(uint))
                + END_FLAG_SIZE;
 
        if (data_out_missing_seq_num_sdram_address != NULL) {
            sdram_free(data_out_missing_seq_num_sdram_address);
            data_out_missing_seq_num_sdram_address = NULL;
        }
        data_out_missing_seq_num_sdram_address = sdram_alloc(size_of_data);
 
        // if not got enough sdram to alllocate all missing seq nums
        if (data_out_missing_seq_num_sdram_address == NULL) {
            // biggest sdram block
            uint32_t max_bytes = sdram_max_block_size();
            // if can't hold more than this packets worth of data, blow up
            if (max_bytes < SDP_PAYLOAD_BYTES + END_FLAG_SIZE) {
                io_printf(IO_BUF,
                        "[ERROR] Can't allocate SDRAM for missing seq nums\n");
                rt_error(RTE_SWERR);
            }
#ifdef DEBUG_DATA_OUT
            io_printf(IO_BUF, "[DEBUG] Activate bacon protocol!\n");
#endif
            // allocate biggest block
            data_out_missing_seq_num_sdram_address = sdram_alloc(max_bytes);
            // determine max full seq num packets to store
            max_bytes -= END_FLAG_SIZE + SDP_PAYLOAD_BYTES;
            data_out_n_missing_seq_packets = 1
                    + max_bytes / (ITEMS_PER_DATA_PACKET * sizeof(uint));
        }
        start_reading_offset = START_OF_MISSING_SEQ_NUMS;
    }
    if (data_out_n_missing_seq_packets > 0) {
        // write data to SDRAM and update packet counter
        data_out_write_missing_seq_nums_into_sdram(
                data, length, start_reading_offset);
        data_out_n_missing_seq_packets--;
#ifdef DEBUG_DATA_OUT
    } else {
        io_printf(IO_BUF, "[WARNING] Unable to save missing sequence number\n");
#endif
    }
}
 
static void data_out_retransmission_dma_read(void) {
    // locate where we are in SDRAM
    address_t data_sdram_position =
            &data_out_missing_seq_num_sdram_address[data_out_position_for_retransmission];
 
    // set off DMA
    data_out_start_dma_read(DMA_TAG_READ_FOR_RETRANSMISSION,
            data_sdram_position, data_out_retransmit_seq_nums,
            ITEMS_PER_DATA_PACKET);
}
 
static void data_out_dma_complete_read_missing_seqeuence_nums(void) {
    // check if at end of read missing sequence numbers
    if (data_out_read_data_position > ITEMS_PER_DATA_PACKET) {
        data_out_position_for_retransmission += ITEMS_PER_DATA_PACKET;
        if (data_out_n_missing_seq_nums_in_sdram >
                data_out_position_for_retransmission) {
            data_out_read_data_position = 0;
            data_out_retransmission_dma_read();
        }
        return;
    }
 
    // get next sequence number to regenerate
    data_out_missing_seq_num_being_processed = (uint32_t)
            data_out_retransmit_seq_nums[data_out_read_data_position];
    if (data_out_missing_seq_num_being_processed != END_FLAG) {
        // regenerate data
        data_out_position_in_store =
                data_out_missing_seq_num_being_processed * SDP_PAYLOAD_WORDS;
        uint32_t left_over_portion =
                data_out_n_elements_to_read_from_sdram -
                data_out_position_in_store;
 
        if (left_over_portion < SDP_PAYLOAD_WORDS) {
            data_out_retransmitted_seq_num_items_read = left_over_portion + 1;
            data_out_read(DMA_TAG_RETRANSMISSION_READING, 1, left_over_portion);
        } else {
            data_out_retransmitted_seq_num_items_read =
                    ITEMS_PER_DATA_PACKET - TRANSACTION_ID_SIZE;
            data_out_read(DMA_TAG_RETRANSMISSION_READING, 1, SDP_PAYLOAD_WORDS);
        }
    } else {        // finished data send, tell host its done
        data_out_send_end_flag();
        data_out_in_retransmission_mode = false;
        if (data_out_missing_seq_num_sdram_address != NULL) {
            sdram_free(data_out_missing_seq_num_sdram_address);
            data_out_missing_seq_num_sdram_address = NULL;
        }
        data_out_read_data_position = 0;
        data_out_position_for_retransmission = 0;
        data_out_n_missing_seq_nums_in_sdram = 0;
    }
}
 
static void data_out_dma_complete_reading_retransmission_data(void) {
    // set sequence number as first element
    data_out_data_to_transmit[data_out_transmit_dma_pointer][0] =
            data_out_missing_seq_num_being_processed;
#ifdef DEBUG_DATA_OUT
    if (data_out_missing_seq_num_being_processed > data_out_max_seq_num) {
        io_printf(IO_BUF,
                "[WARNING] Got some bad seq num here; max is %d, got %d\n",
                data_out_max_seq_num, data_out_missing_seq_num_being_processed);
    }
#endif
 
    // send new data back to host
    data_out_send_data_block(
            data_out_transmit_dma_pointer,
            data_out_retransmitted_seq_num_items_read,
            data_out_new_sequence_key, data_out_basic_data_key);
 
    data_out_read_data_position++;
    data_out_dma_complete_read_missing_seqeuence_nums();
}
 
static void data_out_dma_complete_writing_missing_seq_to_sdram(void) {
#ifdef DEBUG_DATA_OUT
    io_printf(IO_BUF, "[INFO] Need to figure what to do here\n");
#endif
}
 
static void data_out_speed_up_command(sdp_msg_pure_data *msg) {
    sdp_data_out_t *message = (sdp_data_out_t *) msg->data;
 
    switch (message->command) {
    case SDP_CMD_START_SENDING_DATA: {
        // updater transaction id if it hits the cap
        if (((data_out_transaction_id + 1) & TRANSACTION_CAP) == 0) {
            data_out_transaction_id = 0;
            publish_transaction_id(data_out_transaction_id);
        }
 
        // if transaction id is not as expected. ignore it as its from the past.
        // and worthless
        if (message->transaction_id != data_out_transaction_id + 1) {
            io_printf(IO_BUF,
                    "[WARNING] received start message with unexpected "
                    "transaction id %d; mine is %d\n",
                    message->transaction_id, data_out_transaction_id + 1);
            return;
        }
 
        //extract transaction id and update
        data_out_transaction_id = message->transaction_id;
        publish_transaction_id(data_out_transaction_id);
 
        data_out_stop = false;
 
        // set SDRAM position and length
        data_out_store_address = message->sdram_location;
        // state for how many bytes it needs to send, gives approximate
        // bandwidth if round number.
        uint32_t bytes_to_read_write = message->length;
 
        data_out_max_seq_num = bytes_to_read_write / SDP_PAYLOAD_BYTES;
        uint32_t mod = bytes_to_read_write % SDP_PAYLOAD_BYTES;
        data_out_max_seq_num += mod > 0;
 
        // reset states
        data_out_first_transmission = true;
        data_out_transmit_dma_pointer = 0;
        data_out_position_in_store = 0;
        data_out_n_elements_to_read_from_sdram =
                bytes_to_read_write / sizeof(uint);
 
        if (data_out_n_elements_to_read_from_sdram < SDP_PAYLOAD_WORDS) {
            data_out_read(DMA_TAG_READ_FOR_TRANSMISSION, 2,
                    data_out_n_elements_to_read_from_sdram);
        } else {
            data_out_read(DMA_TAG_READ_FOR_TRANSMISSION, 2, SDP_PAYLOAD_WORDS);
        }
        prov->n_out_streams++;
        return;
    }
    case SDP_CMD_START_OF_MISSING_SDP_PACKETS:
        if (message->transaction_id != data_out_transaction_id) {
            io_printf(IO_BUF,
                    "[WARNING] received data from a different transaction for "
                    "start of missing. expected %d got %d\n",
                    data_out_transaction_id, message->transaction_id);
            return;
        }
 
        // if already in a retransmission phase, don't process as normal
        if (data_out_n_missing_seq_packets != 0) {
#ifdef DEBUG_DATA_OUT
            io_printf(IO_BUF, "[INFO] forcing start of retransmission packet\n");
#endif
            data_out_n_missing_seq_packets = 0;
            data_out_missing_seq_num_sdram_address[data_out_n_missing_seq_nums_in_sdram++] =
                    END_FLAG;
            data_out_read_data_position = 0;
            data_out_position_for_retransmission = 0;
            data_out_in_retransmission_mode = true;
            data_out_retransmission_dma_read();
            return;
        }
        // fall through
    case SDP_CMD_MORE_MISSING_SDP_PACKETS:
        if (message->transaction_id != data_out_transaction_id) {
            io_printf(IO_BUF,
                    "[WARNING] received data from different transaction for "
                    "more missing; expected %d, got %d\n",
                    data_out_transaction_id, message->transaction_id);
            return;
        }
 
        // reset state, as could be here from multiple attempts
        if (!data_out_in_retransmission_mode) {
            // put missing sequence numbers into SDRAM
            data_out_store_missing_seq_nums(
                    msg->data,
                    (msg->length - LENGTH_OF_SDP_HEADER) / sizeof(uint),
                    message->command == SDP_CMD_START_OF_MISSING_SDP_PACKETS);
 
            // if got all missing packets, start retransmitting them to host
            if (data_out_n_missing_seq_packets == 0) {
                // packets all received, add finish flag for DMA stoppage
 
                if (data_out_n_missing_seq_nums_in_sdram != 0) {
                    data_out_missing_seq_num_sdram_address[
                            data_out_n_missing_seq_nums_in_sdram++] = END_FLAG;
                    data_out_read_data_position = 0;
                    data_out_position_for_retransmission = 0;
 
                    // start DMA off
                    data_out_in_retransmission_mode = true;
                    data_out_retransmission_dma_read();
                }
            }
        }
        return;
    case SDP_CMD_CLEAR:
        if (message->transaction_id != data_out_transaction_id) {
            io_printf(IO_BUF,
                    "[WARNING] received data from different transaction for "
                    "clear; expected %d, got %d\n",
                    data_out_transaction_id, message->transaction_id);
            return;
        }
#ifdef DEBUG_DATA_OUT
        io_printf(IO_BUF, "[INFO] data out clear\n");
#endif
        data_out_stop = true;
        break;
    default:
        io_printf(IO_BUF, "[WARNING] Received unknown SDP packet: %d\n",
                message->command);
    }
}
 
static INT_HANDLER data_out_dma_complete(void) {
    // reset the interrupt.
    dma_control->control = (dma_control_t) {
        .clear_done_int = true
    };
    if (!data_out_stop) {
        // Only do something if we have not been told to stop
        switch (data_out_dma_port_last_used) {
        case DMA_TAG_READ_FOR_TRANSMISSION:
            data_out_dma_complete_reading_for_original_transmission();
            break;
        case DMA_TAG_READ_FOR_RETRANSMISSION:
            data_out_dma_complete_read_missing_seqeuence_nums();
            break;
        case DMA_TAG_RETRANSMISSION_READING:
            data_out_dma_complete_reading_retransmission_data();
            break;
        case DMA_TAG_FOR_WRITING_MISSING_SEQ_NUMS:
            data_out_dma_complete_writing_missing_seq_to_sdram();
            break;
        default:
            io_printf(IO_BUF, "[ERROR] Invalid DMA callback port: %d\n",
                    data_out_dma_port_last_used);
            rt_error(RTE_SWERR);
        }
    }
    // and tell VIC we're done
    vic_interrupt_done();
}
 
static INT_HANDLER data_out_dma_error(void) {
    io_printf(IO_BUF, "[WARNING] DMA failed: 0x%08x\n", dma_control->status);
    dma_control->control = (dma_control_t) {
        // Clear the error
        .restart = true
    };
    vic_interrupt_done();
    rt_error(RTE_DABT);
}
 
static INT_HANDLER data_out_dma_timeout(void) {
    io_printf(IO_BUF, "[WARNING] DMA timeout: 0x%08x\n", dma_control->status);
    dma_control->control = (dma_control_t) {
        .clear_timeout_int = true
    };
    vic_interrupt_done();
}
 
//-----------------------------------------------------------------------------
// common code
//-----------------------------------------------------------------------------
 
void __real_sark_int(void *pc);
// This function is why this code *can't* use Spin1API.
void __wrap_sark_int(void *pc) {
    // Get the message from SCAMP and see if t belongs to SARK
    if (sark.vcpu->mbox_ap_cmd != SHM_MSG) {
        // Run the default callback
        __real_sark_int(pc);
        return;
    }
 
    // Make a copy so we can release the mailbox, and flag as ready for
    // interrupt again
    sdp_msg_t *msg = get_message_from_mailbox();
    //io_printf(IO_BUF, "seq is %d\n", msg->seq);
    system_control->clear_cpu_irq = (sc_magic_proc_map_t) {
        .security_code = SYSTEM_CONTROLLER_MAGIC_NUMBER,
        .select = 1 << sark.phys_cpu,
    };
    if (msg == NULL) {
        return;
    }
 
    switch ((msg->dest_port & PORT_MASK) >> PORT_SHIFT) {
    case REINJECTION_PORT:
        reflect_sdp_message(msg, reinjection_sdp_command(msg));
        while (!sark_msg_send(msg, 10)) {
#ifdef DEBUG_REINJECTOR
            io_printf(IO_BUF, "[DEBUG] timeout when sending reinjection reply\n");
#endif
        }
        prov->n_sdp_packets++;
        break;
    case DATA_SPEED_UP_OUT_PORT:
        // These are all one-way messages; replies are out of band
        data_out_speed_up_command((sdp_msg_pure_data *) msg);
        prov->n_sdp_packets++;
        break;
    case DATA_SPEED_UP_IN_PORT:
        reflect_sdp_message(msg, data_in_speed_up_command(msg));
        while (!sark_msg_send(msg, 10)) {
#ifdef DEBUG_DATA_IN
            io_printf(IO_BUF, "[DEBUG] timeout when sending speedup ctl reply\n");
#endif
        }
        prov->n_sdp_packets++;
        break;
    default:
        io_printf(IO_BUF, "[WARNING] unexpected port %d\n",
                (msg->dest_port & PORT_MASK) >> PORT_SHIFT);
        io_printf(IO_BUF,
                "[INFO] from:%04x:%02x to:%04x:%02x cmd:%04x len:%d iam:%04x\n",
                msg->srce_addr, msg->srce_port,
                msg->dest_addr, msg->dest_port,
                msg->cmd_rc, msg->length, my_addr);
        // Do nothing
    }
    sark_msg_free(msg);
}
 
//-----------------------------------------------------------------------------
// initializers
//-----------------------------------------------------------------------------
 
static void reinjection_initialise(void) {
    // set up config region
    // Get the address this core's DTCM data starts at from SRAM
    reinjection_read_packet_types(dse_block(CONFIG_REINJECTION));
 
    // Setup the CPU interrupt for WDOG
    vic_interrupt_control[sark_vec->sark_slot] = (vic_vector_control_t) {
        .enable = false
    };
    set_vic_callback(CPU_SLOT, CPU_INT, sark_int_han);
 
    // Setup the communications controller interrupt
    set_vic_callback(CC_SLOT, CC_TNF_INT, reinjection_ready_to_send_callback);
 
    // Setup the timer interrupt
    set_vic_callback(TIMER_SLOT, TIMER1_INT, reinjection_timer_callback);
 
    // Setup the router interrupt as a fast interrupt
    sark_vec->fiq_vec = reinjection_dropped_packet_callback;
    vic_control->int_select = (vic_mask_t) {
        .router_dump = true
    };
}
 
static void data_out_initialise(void) {
    data_speed_out_config_t *config = dse_block(CONFIG_DATA_SPEED_UP_OUT);
    data_out_basic_data_key = config->my_key;
    data_out_new_sequence_key = config->new_seq_key;
    data_out_first_data_key = config->first_data_key;
    data_out_transaction_id_key = config->transaction_id_key;
    data_out_end_flag_key = config->end_flag_key;
 
#ifdef DEBUG_DATA_OUT
    io_printf(IO_BUF,
            "[INFO] new seq key = %d, first data key = %d, transaction id key = %d, "
            "end flag key = %d, basic_data_key = %d\n",
            data_out_new_sequence_key, data_out_first_data_key, data_out_transaction_id_key,
            data_out_end_flag_key, data_out_basic_data_key);
#endif
 
    // Various DMA callbacks
    set_vic_callback(DMA_SLOT, DMA_DONE_INT, data_out_dma_complete);
    set_vic_callback(DMA_ERROR_SLOT, DMA_ERR_INT, data_out_dma_error);
    set_vic_callback(DMA_TIMEOUT_SLOT, DMA_TO_INT, data_out_dma_timeout);
 
    // configuration for the DMA's by the speed data loader
    dma_control->control = (dma_control_t) {
        // Abort pending and active transfers
        .uncommit = true,
        .abort = true,
        .restart = true,
        .clear_done_int = true,
        .clear_timeout_int = true,
        .clear_write_buffer_int = true
    };
    dma_control->control = (dma_control_t) {
        // clear possible transfer done and restart
        .uncommit = true,
        .restart = true,
        .clear_done_int = true
    };
    dma_control->global_control = (dma_global_control_t) {
        // enable DMA done and error interrupt
        .transfer_done_interrupt = true,
        .transfer2_done_interrupt = true,
        .timeout_interrupt = true,
        .crc_error_interrupt = true,
        .tcm_error_interrupt = true,
        .axi_error_interrupt = true, // SDRAM error
        .user_abort_interrupt = true,
        .soft_reset_interrupt = true,
        .write_buffer_error_interrupt = true
    };
}
 
static void data_in_initialise(void) {
    data_in_saved_application_router_table = sdram_alloc(
            N_USABLE_ROUTER_ENTRIES * sizeof(router_entry_t));
    if (data_in_saved_application_router_table == NULL) {
        io_printf(IO_BUF,
                "[ERROR] failed to allocate SDRAM for application mc router entries\n");
        rt_error(RTE_SWERR);
    }
 
    data_in_data_items_t *items = dse_block(CONFIG_DATA_SPEED_UP_IN);
 
    data_in_address_key = items->address_mc_key;
    data_in_data_key = items->data_mc_key;
    data_in_boundary_key = items->boundary_mc_key;
    // Save the current (application?) state
    data_in_save_router();
 
    // load user 1 in case this is a consecutive load
    publish_transaction_id(data_out_transaction_id);
 
    // set up mc interrupts to deal with data writing
    set_vic_callback(MC_PAYLOAD_SLOT, CC_MC_INT, process_mc_payload_packet);
}
 
static void provenance_initialise(void) {
    prov = dse_block(PROVENANCE_REGION);
    prov->n_sdp_packets = 0;
    prov->n_in_streams = 0;
    prov->n_out_streams = 0;
    prov->n_router_changes = 0;
}
 
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
void c_main(void) {
    sark_cpu_state(CPU_STATE_RUN);
 
    dse_regions = data_specification_get_data_address();
    if (!data_specification_read_header(dse_regions)) {
        rt_error(RTE_SWERR);
    }
 
    // Configure
    my_addr = sv->p2p_addr;
    reinjection_configure_timer();
    reinjection_configure_comms_controller();
    reinjection_configure_router();
 
    // Initialise the statistics
    reinject_n_dropped_packets = 0;
    reinject_n_reinjected_packets = 0;
    reinject_n_missed_dropped_packets = 0;
    reinject_n_dropped_packet_overflows = 0;
 
    // set up VIC callbacks and interrupts accordingly
    // Disable the interrupts that we are configuring (except CPU for WDOG)
    const vic_mask_t int_select = {
        .timer1 = true,
        .router_dump = true,
        .dma_done = true,
        .dma_error = true,
        .dma_timeout = true,
        .cc_rx_mc = true,
    };
    vic_control->int_disable = int_select;
    reinjection_disable_comms_interrupt();
 
    // set up provenance area
    provenance_initialise();
 
    // set up reinjection functionality
    reinjection_initialise();
 
    // set up data speed up functionality
    data_out_initialise();
    data_in_initialise();
 
    // Enable interrupts and timer
    vic_control->int_enable = int_select;
    timer1_control->control = (timer_control_t) {
        .size = 1,
        .interrupt_enable = true,
        .periodic_mode = true,
        .enable = true
    };
 
    io_printf(IO_BUF, "[INFO] extra monitor initialisation complete\n");
 
    // Run until told to exit
    while (reinject_run) {
        wait_for_interrupt();
    }
}
// ------------------------------------------------------------------------