New branch with firmware dir from USRP2P branch.

1 files changed, 251 insertions, 0 deletions

diff --git a/firmware/microblaze/apps/cruft/app_passthru_v2.c b/firmware/microblaze/apps/cruft/app_passthru_v2.c
new file mode 100644
index 000000000..406c56b3b
--- /dev/null
+++ b/firmware/microblaze/apps/cruft/app_passthru_v2.c
@@ -0,0 +1,251 @@
+/* -*- c++ -*- */
+/*
+ * Copyright 2007,2008 Free Software Foundation, Inc.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include "app_passthru_v2.h"
+#include "buffer_pool.h"
+#include "memcpy_wa.h"
+#include "ethernet.h"
+#include "nonstdio.h"
+#include "print_rmon_regs.h"
+#include "db.h"
+#include "clocks.h"
+#include <string.h>
+
+volatile bool link_is_up = false;	// eth handler sets this
+
+
+// If this is non-zero, this dbsm could be writing to the ethernet
+dbsm_t *ac_could_be_sending_to_eth;
+
+//static unsigned char exp_seqno = 0;
+
+void
+set_reply_hdr(u2_eth_packet_t *reply_pkt, u2_eth_packet_t const *cmd_pkt)
+{
+  reply_pkt->ehdr.dst = cmd_pkt->ehdr.src;
+  reply_pkt->ehdr.src = *ethernet_mac_addr();
+  reply_pkt->ehdr.ethertype = U2_ETHERTYPE;
+  reply_pkt->thdr.flags = 0;
+  reply_pkt->thdr.fifo_status = 0;	// written by protocol engine
+  reply_pkt->thdr.seqno = 0;		// written by protocol engine
+  reply_pkt->thdr.ack = 0;		// written by protocol engine
+  u2p_set_word0(&reply_pkt->fixed, 0, CONTROL_CHAN);
+  reply_pkt->fixed.timestamp = timer_regs->time;
+}
+
+static void
+send_reply(unsigned char *reply, size_t reply_len)
+{
+  if (reply_len < 64)
+    reply_len = 64;
+
+  // wait for buffer to become idle
+  hal_set_leds(0x4, 0x4);
+  while((buffer_pool_status->status & BPS_IDLE(CPU_TX_BUF)) == 0)
+    ;
+  hal_set_leds(0x0, 0x4);
+
+  // copy reply into CPU_TX_BUF
+  memcpy_wa(buffer_ram(CPU_TX_BUF), reply, reply_len);
+
+  // wait until nobody else is sending to the ethernet
+  if (ac_could_be_sending_to_eth){
+    hal_set_leds(0x8, 0x8);
+    dbsm_wait_for_opening(ac_could_be_sending_to_eth);
+    hal_set_leds(0x0, 0x8);
+  }
+
+  // fire it off
+  bp_send_from_buf(CPU_TX_BUF, PORT_ETH, 1, 0, reply_len/4);
+
+  // wait for it to complete (not long, it's a small pkt)
+  while((buffer_pool_status->status & (BPS_DONE(CPU_TX_BUF) | BPS_ERROR(CPU_TX_BUF))) == 0)
+    ;
+
+  bp_clear_buf(CPU_TX_BUF);
+}
+
+
+static size_t
+op_id_cmd(const op_generic_t *p,
+	  void *reply_payload, size_t reply_payload_space)
+{
+  op_id_reply_t *r = (op_id_reply_t *) reply_payload;
+  if (reply_payload_space < sizeof(*r))	// no room
+    return 0;
+
+  // Build reply subpacket
+
+  r->opcode = OP_ID_REPLY;
+  r->len = sizeof(op_id_reply_t);
+  r->rid = p->rid;
+  r->addr = *ethernet_mac_addr();
+  r->hw_rev = 0x0000;	// FIXME
+  // r->fpga_md5sum = ;	// FIXME
+  // r->sw_md5sum = ;	// FIXME
+
+  // FIXME Add d'board info, including dbid, min/max gain, min/max freq
+
+  return r->len;
+}
+
+static size_t
+add_eop(void *reply_payload, size_t reply_payload_space)
+{
+  op_generic_t *r = (op_generic_t *) reply_payload;
+  if (reply_payload_space < sizeof(*r))		
+    return 0;					// no room
+
+  r->opcode = OP_EOP;
+  r->len = sizeof(*r);
+  r->rid = 0;
+  r->ok =  0;
+
+  return r->len;
+}
+
+bool
+handle_control_chan_frame(u2_eth_packet_t *pkt, size_t len)
+{
+  unsigned char reply[sizeof(u2_eth_packet_t) + 4 * sizeof(u2_subpkt_t)] _AL4;
+  unsigned char *reply_payload = &reply[sizeof(u2_eth_packet_t)];
+  int reply_payload_space = sizeof(reply) - sizeof(u2_eth_packet_t);
+
+  bool handled_it = false;
+
+  // initialize reply
+  memset(reply, 0, sizeof(reply));
+  set_reply_hdr((u2_eth_packet_t *) reply, pkt);
+
+  // point to beginning of payload (subpackets)
+  unsigned char *payload = ((unsigned char *) pkt) + sizeof(u2_eth_packet_t);
+  int payload_len = len - sizeof(u2_eth_packet_t);
+
+  size_t subpktlen = 0;
+
+  while (payload_len >= sizeof(op_generic_t)){
+    const op_generic_t *gp = (const op_generic_t *) payload;
+    subpktlen = 0;
+
+    switch(gp->opcode){
+    case OP_EOP:		// end of subpackets
+      goto end_of_subpackets;
+
+    case OP_ID:
+      subpktlen = op_id_cmd(gp, reply_payload, reply_payload_space);
+      handled_it = true;
+      break;
+
+    default:
+      if (0){
+	printf("\npassing on %d\n", gp->opcode);
+      }
+      break;
+    }
+
+    int t = (gp->len + 3) & ~3;		// bump to a multiple of 4
+    payload += t;
+    payload_len -= t;
+
+    subpktlen = (subpktlen + 3) & ~3;	// bump to a multiple of 4
+    reply_payload += subpktlen;
+    reply_payload_space -= subpktlen;
+  }
+
+ end_of_subpackets:
+
+  if (handled_it){
+    // add the EOP marker
+    subpktlen = add_eop(reply_payload, reply_payload_space);
+    subpktlen = (subpktlen + 3) & ~3;	// bump to a multiple of 4
+    reply_payload += subpktlen;
+    reply_payload_space -= subpktlen;
+
+    send_reply(reply, reply_payload - reply);
+  }
+
+  return handled_it;
+}
+
+
+/*
+ * Called when an ethernet packet is received.
+ * Return true if we handled it here, otherwise
+ * it'll be passed on to the DSP Tx pipe
+ */
+bool
+eth_pkt_inspector(dbsm_t *sm, int bufno)
+{
+  u2_eth_packet_t *pkt = (u2_eth_packet_t *) buffer_ram(bufno);
+  size_t byte_len = (buffer_pool_status->last_line[bufno] - 3) * 4;
+
+  //static size_t last_len = 0;
+
+  // hal_toggle_leds(0x1);
+
+  // inspect rcvd frame and figure out what do do.
+
+  if (pkt->ehdr.ethertype != U2_ETHERTYPE)
+    return true;	// ignore, probably bogus PAUSE frame from MAC
+
+  int chan = u2p_chan(&pkt->fixed);
+
+  switch (chan){
+  case CONTROL_CHAN:
+    return handle_control_chan_frame(pkt, byte_len);
+    break;
+
+  case 0:
+  default:
+#if 0
+    if (last_len != 0){
+      if (byte_len != last_len){
+	printf("Len: %d last: %d\n", byte_len, last_len);
+      }
+    }
+    last_len = byte_len;
+
+    if((pkt->thdr.seqno) == exp_seqno){
+      exp_seqno++;
+      //putchar('.');
+    }
+    else {
+      // putchar('S');
+      //printf("S%d %d ",exp_seqno,pkt->thdr.seqno);
+      exp_seqno = pkt->thdr.seqno + 1;
+    }
+#endif
+    return false;	// pass it on to Tx DSP
+    break;
+  }
+}
+
+/*
+ * Called when eth phy state changes (w/ interrupts disabled)
+ */
+void
+link_changed_callback(int speed)
+{
+  link_is_up = speed != 0;
+  hal_set_leds(link_is_up ? LED_RJ45 : 0x0, LED_RJ45);
+  printf("\neth link changed: speed = %d\n", speed);
+}