// selfid.c

#include "1394.h"
#include "data_structures.h"
#include "phy_services.h"
#include "arb.h"
#include "shared.h"

void self_ID_start_actions() {
  int i;
  all_child_ports_identified = TRUE;    // Will be reset if any active children are unidentified
  concatenated_packet = FALSE;          // Prepare in case of multiple self_ID packets
  arb_timer = 0;
  for (i = 0; i < NPORT; i++)
    if (child_ID_complete[i])           // Tell identified children to prepare to receive data
      portTarb(i, DATA_PREFIX); 
    else {
      portTarb(i, IDLE);                // Allow parent to finish
      if (child[i] && (active[i] || proxy[i])) {
        if (all_child_ports_identified)
          lowest_unidentified_child = i;
        all_child_ports_identified = FALSE;
      }
    }
}

void self_ID_grant_actions() {
  int i;
  int SID_pkt_number;
  int port_stat;
  int port_number = 0;
  PHY_PKT self_ID;
  loop_to_detect = FALSE;               // no longer looking for loop timeout detect conditions
  for (i = 0; i < NPORT; i++) {
    if (!all_child_ports_identified && (i == lowest_unidentified_child)) {
      if (!proxy[i]) portTarb(i, GRANT); // Send grant to lowest unidentified child (if any)
    } else portTarb(i, DATA_PREFIX);      // Otherwise, tell others to prepare for packet
  }
  if (!all_child_ports_identified && proxy[lowest_unidentified_child]) {

    if ((breq == FAIR_REQ) || (breq == PRIORITY_REQ)) {
      breq = NO_REQ;                      // Cancel the request
      PH_ARB_confirmation(PH_LOST, 0, 0); // And let the link know
    }
    PH_DATA_indication(PH_DATA_PREFIX, 0, 0, 0); // Send notification of bus activity
    for (SID_pkt_number = 0; 
        SID_pkt_number < standby_packet_count[lowest_unidentified_child]; SID_pkt_number++) {
      start_tx_packet(S100, LEGACY);    // Send data prefix and S100 speed code (always Legacy format)
      PH_DATA_indication(PH_DATA_START, S100, 0, LEGACY);
      self_ID.dataQuadlet = 0;          // Clear all zero fields in self_ID packet
      self_ID.phy_pktType = 0b10;
      self_ID.phy_ID = physical_ID;
      standby_phy_ID[lowest_unidentified_child] = physical_ID; // and remember the new PHY_ID
      if (SID_pkt_number == 0) {        // First self ID packet?
        self_ID.L = standby_L[lowest_unidentified_child]; // Link active or not?
        self_ID.gap_cnt = gap_count;
        self_ID.sp = 0b11;              // "PHY_SPEED" always "11" for 1394b nodes
        self_ID.c = standby_c[lowest_unidentified_child];
        self_ID.pwr = standby_pwr[lowest_unidentified_child];
      } else {
        self_ID.ext = 1;                // Indicates second and subsequent packets
        self_ID.n = SID_pkt_number - 1; // Sequence number
      }
      port_stat = 0;                    // Initialize for fresh group of ports
      while (port_number < ((SID_pkt_number + 1) * 8 - 5)) { // Concatenate port status
        if (port_number >= standby_NPORT[lowest_unidentified_child])
          ;                             // Unimplemented
        else if (standby_parent[lowest_unidentified_child] == port_number)
          port_stat |= 0b10;            // Active parent
        else
          port_stat |= 0b01;            // Disabled, disconnected or suspended
        port_number++;
        port_stat <<= 2;                // Make room for next port's status
      }
      self_ID.dataQuadlet |= port_stat;
      if (SID_pkt_number == standby_packet_count[lowest_unidentified_child] - 1) { 
        // Last packet?
        tx_quadlet(self_ID.dataQuadlet);
        tx_quadlet(~self_ID.dataQuadlet);
        // Yes, signal data end
        PH_DATA_indication(PH_DATA_END, 0, 0, 0);
        stop_tx_packet(DATA_END, NPORT);
      } else {
        self_ID.m = 1;                  // Other packets follow, set 'more' bit
        tx_quadlet(self_ID.dataQuadlet);
        tx_quadlet(~self_ID.dataQuadlet);
        // Keep bus for concatenation
        PH_DATA_indication(PH_DATA_PREFIX, 0, 0, 0);
        stop_tx_packet(DATA_PREFIX, NPORT);  
      }
    }

    child_ID_complete[lowest_unidentified_child] = TRUE;
    if (physical_ID < 63)               // Stop at 63 if malconfigured bus
      physical_ID = physical_ID + 1;    // Otherwise, take next PHY address
    idle_receive_port = TRUE;           // to exit back to self_ID start
  } else {
    idle_receive_port = FALSE;
  }
}

void self_ID_receive_actions() {
  int i;
  int SID_pkt_number = 0;
  int port_number = 0;
  int port_stat;
  arb_timer = 0;
  loop_to_detect = FALSE;               // no longer looking for loop timeout detect conditions
  portTarb(receive_port, IDLE);         // Turn off grant, get ready to receive
  
  do {
    receive_actions();                  // Receive (and repeat) packet
    if (SID_pkt_number == 0) {          // Only do this on the first self_ID packet
      // get the self ID info ready for restore from standby
      standby_phy_ID[receive_port] = self_ID_pkt.phy_ID;
      standby_L[receive_port] = self_ID_pkt.L;
      standby_c[receive_port] = self_ID_pkt.c;
      standby_pwr[receive_port] = self_ID_pkt.pwr;
    }
    // find which port the peer is using as parent
    
    while (port_number < ((SID_pkt_number +1)*8 -5)) {
      port_stat = (self_ID_pkt.dataQuadlet>>(((SID_pkt_number +1)*8 -5 -port_number)*2)) & 0b11;
      if (port_stat == 0b10) standby_parent[receive_port] = port_number; // TRUE exactly once
      port_number++;
      if (port_stat != 0b00) standby_NPORT[receive_port] = port_number; 
        // eventually save the highest value
    }
    SID_pkt_number++; 
  } while (concatenated_packet);

  standby_packet_count[receive_port] = SID_pkt_number;
  if (physical_ID < 63)                 // Stop at 63 if malconfigured bus
    physical_ID = physical_ID + 1;      // Otherwise, take next PHY address
  for (i = 0; i < NPORT; i++)
    portTarb(i, IDLE);                  // Turn off all transmitters
  
  // ensure that MIN_IDLE_TIME is preserved before heading off to S0
  arb_timer = 0;
  while ((arb_timer < Legacy_time(MIN_IDLE_TIME)) && 
    !((portRarb[receive_port] == SELF_ID_GRANT) || 
      (portRarb[receive_port] == DATA_PREFIX) ||
      (portRarb[receive_port] == IDENT_DONE)))
    ;
}

void self_ID_transmit_actions() {
  int i;
  int last_SID_pkt = (NPORT + 4) / 8;
  int SID_pkt_number;                   // Packet number counter
  int port_number = 0;                  // Port number counter
  PHY_PKT self_ID;
  int port_stat;

  arb_timer = 0;
  receive_port = NPORT;                 // Indicate that node is transmitting (no port has this number)
  // set B_node_root and max Legacy path speed (done here to be symmetric with 
  // self_ID processing which is done in decode_pky_pkt)
  if (link == Legacy_Link) {
    B_node_root = FALSE;
    B_bus = FALSE;        
  } else B_node_root = TRUE;
  if (border_node) {                    // forget about border nodes that the local node has 
                                        // already heard about
    proxy_root = TRUE;                  // working assumptions, may be changed later
    senior_port = NPORT+1;
    senior_border = TRUE;
  }

  if ((breq == FAIR_REQ) || (breq == PRIORITY_REQ)) {
    breq = NO_REQ;                      // Cancel the request
    PH_ARB_confirmation(PH_LOST, 0, 0); // And let the link know
  }
  // Inform link of new node number as early as possible
  if (!ping_response)
    PH_EVENT_indication(PH_SELF_IDENTITY, physical_ID, root); // Unsolicited Register 0
  PH_DATA_indication(PH_DATA_PREFIX, 0, 0, 0); // Send notification of bus activity
  for (SID_pkt_number = 0; SID_pkt_number <= last_SID_pkt; SID_pkt_number++) {
    start_tx_packet(S100, LEGACY);      // Send data prefix and 98.304 Mbit/sec speed code
                                        // always Legacy format, no speed code if legacy link
    PH_DATA_indication(PH_DATA_START, S100, 0, LEGACY);
    self_ID.dataQuadlet = 0;            // Clear all zero fields in self_ID packet
    self_ID.phy_pktType = 0b10;
    self_ID.phy_ID = physical_ID;
    if (SID_pkt_number == 0) {          // First self ID packet?
      self_ID.L = LPS && lctrl;         // Link active or not?
      self_ID.gap_cnt = gap_count;
      self_ID.sp = 0b11;                // "PHY_SPEED" always "11" for 1394b nodes
      self_ID.c = CONTENDER;
      self_ID.pwr = POWER_CLASS;
      self_ID.i = initiated_reset;
    } else {
      self_ID.ext = 1;                  // Indicates second and subsequent packets
      self_ID.n = SID_pkt_number - 1;   // Sequence number
    }
    port_stat = 0;                      // Initialize for fresh group of ports
    while (port_number < ((SID_pkt_number + 1) * 8 - 5)) { // Concatenate port status
      if (port_number >= NPORT)
        ;                               // Unimplemented
      else if (!active[port_number] && !proxy[port_number])
        port_stat |= 0b01;              // Disabled, disconnected or suspended
      else if (child[port_number])
        port_stat |= 0b11;              // Active child
      else
        port_stat |= 0b10;              // Active parent
      port_number++;
      port_stat <<= 2;                  // Make room for next port's status
    }
    self_ID.dataQuadlet |= port_stat;
    if (SID_pkt_number == last_SID_pkt) { // Last packet?
      tx_quadlet(self_ID.dataQuadlet);
      tx_quadlet(~self_ID.dataQuadlet);
      // Yes, signal data end
      PH_DATA_indication(PH_DATA_END, 0, 0, 0);
      stop_tx_packet(DATA_END, NPORT);
    } else {
      self_ID.m = 1;                    // Other packets follow, set 'more' bit
      tx_quadlet(self_ID.dataQuadlet);
      tx_quadlet(~self_ID.dataQuadlet);
      // Keep bus for concatenation
      PH_DATA_indication(PH_DATA_PREFIX, 0, 0, 0);
      stop_tx_packet(DATA_PREFIX, NPORT);  
    }
  }
  if (!ping_response) {                 // Skip if self_ID packet was in response to a ping
    for (i = 0; i < NPORT; i++) {
      if (root || i != parent_port)
        portTarb(i, IDLE);              // Turn off transmitters to children
      else                              // Notify parent that self_ID is complete
        portTarb(i, IDENT_DONE);  
    }
    if (!root) {                        // If node has a parent...
      // Send speed signal (if any)
      portTspeed_raw(parent_port, DS_port_speed[parent_port]); 
      wait_Legacy_time(SPEED_SIGNAL_LENGTH);
      // Stop sending speed signal
      portTspeed_raw(parent_port, S100);   
    }
    if (root && B_bus) {
      send_async_start_token = TRUE;    // to get everyone in phase
      gap_repeat_actions(TRUE, NPORT);
      OK_to_grant = TRUE;
      defer_grant = FALSE;
    }
  }
}