// speed.c

#define unsubscripted
#include "1394.h"
#include "data_structures.h"
#include "phy_services.h"
#include "shared.h"

void speed_filter() {                   // Continuously sample speed signals
  int i;
  boolean OK_to_sample;
  speedCode raw_speed[2];               // Unfiltered speed (moving window of two samples)
  RX_signal new_signal;

  if (!bias) raw_speed[0] = S100;       // initialize
  for (i = 0; i < (2<<DS_PHY_SPEED); i++) // Wait for 50 MHz clock
    wait_event(PH_DS_BIT_CLOCK);
  new_signal = PMD_DSPORT_SIGNAL_request();                // Get signal
  raw_speed[1] = raw_speed[0];          // Save prior sample
  if (PMD_DSPORT_RXSPEED_request() & RX_S400) {                        // S400 observed?
    if (PMD_DSPORT_RXSPEED_request() & RX_S200)
      raw_speed[0] = S400;
  } else if (PMD_DSPORT_RXSPEED_request() & RX_S200)                   // S200 observed?
    raw_speed[0] = S200;
  else
    raw_speed[0] = S100;                // No to both: default S100
  OK_to_sample = new_signal.RX_arb == RX_CHILD_HANDSHAKE // transmitting TX_IDENT_DONE 
            || new_signal.RX_arb == RX_IDENT_DONE
            || new_signal.RX_arb == RX_DATA_PREFIX;
  if (!OK_to_sample)
    ds_portRspeed = S100;               // Reset to S100 whenever it's not OK to sample
  else if (raw_speed[0] == raw_speed[1]) // Consecutive identical samples?
    if (raw_speed[0] == S200 && ds_portRspeed < S400)
      ds_portRspeed = S200;             // Latch S200 only if S400 not yet confirmed
    else if (raw_speed[0] == S400)
      ds_portRspeed = S400;
}