rework YUV to make it easier to pipeline

Change-Id: Ic8b2a9dd5fded5ce7d3f5833565cea08ca06a1a8

// Copyright (c) 2013 Nokia, Inc.
// Copyright (c) 2013 Quanta Research Cambridge, Inc.

// Permission is hereby granted, free of charge, to any person
// obtaining a copy of this software and associated documentation
// files (the "Software"), to deal in the Software without
// restriction, including without limitation the rights to use, copy,
// modify, merge, publish, distribute, sublicense, and/or sell copies
// of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be
// included in all copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
// EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
// MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
// NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
// BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
// ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
// CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

import Vector::*;
import Arith::*;

// little endian RGB
typedef struct {
    Bit#(8) r;
    Bit#(8) g;
    Bit#(8) b;
} Rgb888 deriving (Bits);

typedef struct {
    Bit#(1) vsync;
    Bit#(1) hsync;
    Bit#(1) de;
    pixelType pixel;
} VideoData#(type pixelType) deriving (Bits);

typedef struct {
    Bit#(8) y;
    Bit#(8) u;
    Bit#(8) v;
} Yuv444 deriving (Bits);

typedef struct {
    Bit#(8) y1;
    Bit#(8) u;
    Bit#(8) y2;
    Bit#(8) v;
} Yuv422 deriving (Bits);

typedef struct {
    Bit#(8) uv;
    Bit#(8) yy;
} Yyuv deriving (Bits);

interface Rgb888ToYuv422;
    method Action putRgb888(Rgb888 rgb888);
    method ActionValue#(Yuv422) getYuv422();
endinterface

function Yuv444 rgbtoyuv(Rgb888 rgb);
    Bit#(16) y = 77*extend(rgb.r) + 150 * extend(rgb.g) + 29 * extend(rgb.b) + 0;
    Bit#(16) u = -43*extend(rgb.r) - 85*extend(rgb.g) + 128*extend(rgb.b) + 128;
    Bit#(16) v = 128*extend(rgb.r) - 107*extend(rgb.g) - 21*extend(rgb.b) + 128;
    return Yuv444 { y: truncate(y>>8), u: truncate(u >> 8), v: truncate(v >> 8) };
endfunction

typedef Vector#(4, Vector#(3, Bit#(16))) Yuv444Intermediates;

function Vector#(3, Bit#(n)) rgbToVector(Rgb888 rgb) provisos (Add#(a__, 8, n));
   Vector#(3, Bit#(n)) vec;
   vec[0] = zeroExtend(rgb.r);
   vec[1] = zeroExtend(rgb.g);
   vec[2] = zeroExtend(rgb.b);
   return vec;
endfunction
function Yuv444 yuv444FromVector(Vector#(3, Bit#(n)) vec) provisos (Add#(a__, 8, n));
   return Yuv444 { y: truncate(vec[0]), u: truncate(vec[1]), v: truncate(vec[2]) };
endfunction

function Yuv444Intermediates rgbToYuvIntermediates(Rgb888 rgb);
   Vector#(3, Vector#(4, Bit#(16))) rgbv = replicate(append(rgbToVector(rgb), replicate(1)));
   Vector#(3, Vector#(4, Bit#(16))) coeffs = replicate(newVector());
   coeffs[0][0] =  77; coeffs[0][1] =  150; coeffs[0][2] =  29; coeffs[0][3] = 0;
   coeffs[1][0] =  43; coeffs[1][1] =  -85; coeffs[1][2] = 128; coeffs[1][3] = (128<<8);
   coeffs[2][0] = 128; coeffs[2][1] = -107; coeffs[2][2] = -21; coeffs[2][3] = (128<<8);
   return transpose(map(uncurry(vmul), zip(rgbv, coeffs)));
endfunction

function Yuv444 yuvIntermediatesToYuv444(Yuv444Intermediates w);
   Vector#(3, Bit#(16)) v0 = vadd(w[0], w[1]);
   Vector#(3, Bit#(16)) v1 = vadd(w[2], w[3]);
   return yuv444FromVector(vrshift(vadd(vadd(w[0], w[1]), vadd(w[2], w[3])), 8));
endfunction

function Yuv422 yuv444toyuv422(Yuv444 yuv0, Yuv444 yuv1);
    Bit#(9) u = (extend(yuv0.u) + extend(yuv1.u)) >> 1;
    Bit#(9) v = (extend(yuv0.u) + extend(yuv1.v)) >> 1;
    return Yuv422 { y1: yuv0.y, u: truncate(u), y2: yuv1.y, v: truncate(v) };
endfunction

// Copyright (c) 2013 Nokia, Inc.
// Copyright (c) 2013 Quanta Research Cambridge, Inc.

// Permission is hereby granted, free of charge, to any person
// obtaining a copy of this software and associated documentation
// files (the "Software"), to deal in the Software without
// restriction, including without limitation the rights to use, copy,
// modify, merge, publish, distribute, sublicense, and/or sell copies
// of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be
// included in all copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
// EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
// MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
// NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
// BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
// ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
// CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

// little endian RGB
typedef struct {
    Bit#(8) r;
    Bit#(8) g;
    Bit#(8) b;
} Rgb888 deriving (Bits);

typedef struct {
    Bit#(1) vsync;
    Bit#(1) hsync;
    Bit#(1) de;
    pixelType pixel;
} VideoData#(type pixelType) deriving (Bits);

typedef struct {
    Bit#(8) y;
    Bit#(8) u;
    Bit#(8) v;
} Yuv444 deriving (Bits);

typedef struct {
    Bit#(8) y1;
    Bit#(8) u;
    Bit#(8) y2;
    Bit#(8) v;
} Yuv422 deriving (Bits);

typedef struct {
    Bit#(8) uv;
    Bit#(8) yy;
} Yyuv deriving (Bits);

interface Rgb888ToYuv422;
    method Action putRgb888(Rgb888 rgb888);
    method ActionValue#(Yuv422) getYuv422();
endinterface

function Yuv444 rgbtoyuv(Rgb888 rgb);
    Bit#(16) y = 77*extend(rgb.r) + 150 * extend(rgb.g) + 29 * extend(rgb.b) + 0;
    Bit#(16) u = -43*extend(rgb.r) - 85*extend(rgb.g) + 128*extend(rgb.b) + 128;
    Bit#(16) v = 128*extend(rgb.r) - 107*extend(rgb.g) - 21*extend(rgb.b) + 128;
    return Yuv444 { y: truncate(y>>8), u: truncate(u >> 8), v: truncate(v >> 8) };
endfunction

typedef struct {
    Bit#(16) y1;
    Bit#(16) y2;
    Bit#(16) y3;
    Bit#(16) u1;
    Bit#(16) u2;
    Bit#(16) u3;
    Bit#(16) v1;
    Bit#(16) v2;
    Bit#(16) v3;
} Yuv444Intermediates deriving (Bits);

function Yuv444Intermediates rgbToYuvIntermediates(Rgb888 rgb);
    return Yuv444Intermediates {
               y1: 77*extend(rgb.r), y2: 150 * extend(rgb.g), y3: 29 * extend(rgb.b),
               u1: 43*extend(rgb.r), u2: 85*extend(rgb.g), u3: 128*extend(rgb.b),
               v1: 128*extend(rgb.r), v2: 107*extend(rgb.g), v3: 21*extend(rgb.b) };
endfunction

function Yuv444 yuvIntermediatesToYuv444(Yuv444Intermediates w);
    let y = 0 + w.y1 + w.y2 + w.y3;
    let u = (128<<8) - w.u1 -w.u2 + w.u3;
    let v = (128<<8) + w.v1 - w.v2 - w.v3;
    return Yuv444 { y: truncate(y>>8), u: truncate(u >> 8), v: truncate(v >> 8) };
endfunction

function Yuv422 yuv444toyuv422(Yuv444 yuv0, Yuv444 yuv1);
    Bit#(9) u = (extend(yuv0.u) + extend(yuv1.u)) >> 1;
    Bit#(9) v = (extend(yuv0.u) + extend(yuv1.v)) >> 1;
    return Yuv422 { y1: yuv0.y, u: truncate(u), y2: yuv1.y, v: truncate(v) };
endfunction

// module mkRgb888ToYuv422(Rgb888ToYuv422);
//     FIFOF#(Yuv444) yuvFifo <- mkBypassFIFOF();
//     Reg#(Maybe#(Yuv444)) yuvReg1 <- mkReg(tagged Invalid);

//     rule alternating if (yuvReg1 matches tagged Invalid);
//         yuvReg1 <= tagged Valid yuv0;
//     endrule

//     method Action putRgb888(Rgb888 rgb888) if (yuvReg0 matches tagged Invalid);
//         yuvFifo.enq(rgbtoyub(rgb888));
//     endmethod

//     method ActionValue#(Yuv422) getYuv422() if (yuvReg1 matches tagged Valid .yuv1);
//         let yuv0 = yuvFifo.first;
//         yuvFifo.deq;
//         let yuv1 = yuvReg1;
//         yuvReg1 <= tagged Invalid;
//         let yuv422 = yuv444toyuv422(yuv0, yuv1);
//         return yuv422;
//     endmethod

// endmodule


corresponding simplification of mkTLPArbiter

// Copyright (c) 2008- 2009 Bluespec, Inc.  All rights reserved.
// $Revision$
// $Date$
// Copyright (c) 2013 Quanta Research Cambridge, Inc.

// PCI-Express for Xilinx 7
// FPGAs.

package PcieSplitter;

// This is a package which acts as a bridge between a TLP-based PCIe
// interface on one side and an AXI slave (portal) and AXI Master on
// the other.

import GetPut       :: *;
import Connectable  :: *;
import Vector       :: *;
import FIFO         :: *;
import FIFOF        :: *;
import Counter      :: *;
import PCIE         :: *;
import Clocks       :: *;
import ClientServer :: *;

Integer portConfig = 0;
Integer portPortal = 1;
Integer portAxi    = 2;
typedef 3 PortMax;

// When TLP packets come in from the PCIe bus, they are dispatched to
// either the configuration register block, the portal (AXI slave) or
// the AXI master.
interface TLPDispatcher;
   // TLPs in from PCIe
   interface Put#(TLPData#(16)) inFromBus;
   // TLPs out to the bridge implementation
   interface Vector#(PortMax, Get#(TLPData#(16))) out;
endinterface: TLPDispatcher

typedef function Bool tlpMatchFunction(TLPData#(16) tlp) TlpMatchFunction;

(* synthesize *)
module mkTLPDispatcher(TLPDispatcher);
   FIFO#(TLPData#(16))  tlp_in_fifo     <- mkFIFO();
   Vector#(PortMax, FIFOF#(TLPData#(16))) tlp_out_fifo <- replicateM(mkGFIFOF(True,False)); // unguarded enq
   Reg#(Maybe#(Bit#(TLog#(PortMax)))) routeToPort <- mkReg(tagged Invalid);

   PulseWire is_read       <- mkPulseWire();
   PulseWire is_write      <- mkPulseWire();
   PulseWire is_completion <- mkPulseWire();

   function Bool configMatch(TLPData#(16) tlp);
      TLPMemoryIO3DWHeader hdr_3dw = unpack(tlp.data);
      Bool is_config_read    =  tlp.sof
                             && (tlp.hit == 7'h01)
                             && (hdr_3dw.format == MEM_READ_3DW_NO_DATA)
                             ;
      Bool is_config_write   =  tlp.sof
                             && (tlp.hit == 7'h01)
                             && (hdr_3dw.format == MEM_WRITE_3DW_DATA)
                             && (hdr_3dw.pkttype != COMPLETION)
                             ;
      return is_config_read || is_config_write;
   endfunction

   function Bool axiMatch(TLPData#(16) tlp);
      TLPMemoryIO3DWHeader hdr_3dw = unpack(tlp.data);
      Bool is_axi_read       =  tlp.sof
                             && (tlp.hit == 7'h04)
                             && (hdr_3dw.format == MEM_READ_3DW_NO_DATA)
                             ;
      Bool is_axi_write      =  tlp.sof
                             && (tlp.hit == 7'h04)
                             && (hdr_3dw.format == MEM_WRITE_3DW_DATA)
                             && (hdr_3dw.pkttype != COMPLETION)
                             ;
      return is_axi_read || is_axi_write;
   endfunction

   function Bool axiCompletionMatch(TLPData#(16) tlp);
      TLPMemoryIO3DWHeader hdr_3dw = unpack(tlp.data);
      Bool is_axi_completion =  tlp.sof
                             && (hdr_3dw.format == MEM_WRITE_3DW_DATA)
                             && (hdr_3dw.pkttype == COMPLETION)
                             ;
      return is_axi_completion;
   endfunction

   Vector#(PortMax, TlpMatchFunction) matchFunctions = newVector;
   matchFunctions[portConfig] = configMatch;
   matchFunctions[portPortal] = axiMatch;
   matchFunctions[portAxi]    = axiCompletionMatch;

   (* fire_when_enabled *)
   rule dispatch_incoming_TLP;
      TLPData#(16) tlp = tlp_in_fifo.first();
      TLPMemoryIO3DWHeader hdr_3dw = unpack(tlp.data);

      if (tlp.sof) begin
	 Bool matched = False;
         // route the packet based on this header
	 for (Integer port = 0; port < valueOf(PortMax); port = port+1)
            if (!matched && matchFunctions[port](tlp)) begin
	       matched = True;
               if (tlp_out_fifo[port].notFull()) begin
		  tlp_in_fifo.deq();
		  tlp_out_fifo[port].enq(tlp);
		  if (!tlp.eof)
                     routeToPort <= tagged Valid fromInteger(port);
               end
            end
         if (!matched) begin
            // unknown packet type -- just discard it
            tlp_in_fifo.deq();
         end
         // indicate activity type
         //if (is_config_read)                     is_read.send();
         //if (is_config_write)                    is_write.send();
      end
      else begin
	 if (routeToPort matches tagged Valid .port) begin
            if (tlp_out_fifo[port].notFull()) begin
               tlp_in_fifo.deq();
               tlp_out_fifo[port].enq(tlp);
               if (tlp.eof)
                  routeToPort <= tagged Invalid;
            end
         end
         else begin
            // unknown packet type -- just discard it
            tlp_in_fifo.deq();
         end
      end
   endrule: dispatch_incoming_TLP

   Vector#(PortMax, Get#(TLPData#(16))) outtemp;
   for (Integer i = 0; i < valueOf(PortMax); i=i+1)
       outtemp[i] = toGet(tlp_out_fifo[i]);
   interface out = outtemp;
   interface Put inFromBus    = toPut(tlp_in_fifo);
endmodule: mkTLPDispatcher

// Multiple sources of TLP packets must all share the PCIe bus. There
// is an arbiter which controls which source gets access to the PCIe
// endpoint.

interface TLPArbiter;
   // TLPs out to PCIe
   interface Get#(TLPData#(16)) outToBus;
   // TLPs in from the bridge implementation
   interface Vector#(PortMax, Put#(TLPData#(16))) in;
endinterface: TLPArbiter

(* synthesize *)
module mkTLPArbiter(TLPArbiter);
   FIFO#(TLPData#(16))  tlp_out_fifo     <- mkFIFO();
   Vector#(PortMax, FIFOF#(TLPData#(16))) tlp_in_fifo <- replicateM(mkGFIFOF(False,True)); // unguarded deq
   Reg#(Maybe#(Bit#(TLog#(PortMax)))) routeFrom <- mkReg(tagged Invalid);

   PulseWire is_read       <- mkPulseWire();
   PulseWire is_write      <- mkPulseWire();
   PulseWire is_completion <- mkPulseWire();

   (* fire_when_enabled *)
   rule arbitrate_outgoing_TLP;
      if (routeFrom matches tagged Valid .port) begin
         if (tlp_in_fifo[port].notEmpty()) begin
            TLPData#(16) tlp <- toGet(tlp_in_fifo[port]).get();
            tlp_out_fifo.enq(tlp);
            if (tlp.eof)
               routeFrom <= tagged Invalid;
         end
      end
      else begin
	 Bool sentOne = False;
	 for (Integer port = 0; port < valueOf(PortMax); port = port+1) begin
	    if (!sentOne && tlp_in_fifo[port].notEmpty()) begin
               TLPData#(16) tlp <- toget(tlp_in_fifo[port]).get();
	       sentOne = True;
               if (tlp.sof) begin
		  tlp_out_fifo.enq(tlp);
		  if (!tlp.eof)
		     routeFrom <= tagged Valid port;
               end
	    end
	 end
      end
   endrule: arbitrate_outgoing_TLP

   Vector#(PortMax, Put#(TLPData#(16))) intemp;
   for (Integer i = 0; i < valueOf(PortMax); i=i+1)
       intemp[i] = toPut(tlp_in_fifo[i]);
   interface in = intemp;
   interface Get outToBus     = toGet(tlp_out_fifo);
endmodule
endpackage: PcieSplitter

// Copyright (c) 2008- 2009 Bluespec, Inc.  All rights reserved.
// $Revision$
// $Date$
// Copyright (c) 2013 Quanta Research Cambridge, Inc.

// PCI-Express for Xilinx 7
// FPGAs.

package PcieSplitter;

// This is a package which acts as a bridge between a TLP-based PCIe
// interface on one side and an AXI slave (portal) and AXI Master on
// the other.

import GetPut       :: *;
import Connectable  :: *;
import Vector       :: *;
import FIFO         :: *;
import FIFOF        :: *;
import Counter      :: *;
import PCIE         :: *;
import Clocks       :: *;
import ClientServer :: *;

Integer portConfig = 0;
Integer portPortal = 1;
Integer portAxi    = 2;
typedef 3 PortMax;

// When TLP packets come in from the PCIe bus, they are dispatched to
// either the configuration register block, the portal (AXI slave) or
// the AXI master.
interface TLPDispatcher;
   // TLPs in from PCIe
   interface Put#(TLPData#(16)) inFromBus;
   // TLPs out to the bridge implementation
   interface Vector#(PortMax, Get#(TLPData#(16))) out;
endinterface: TLPDispatcher

typedef function Bool tlpMatchFunction(TLPData#(16) tlp) TlpMatchFunction;

(* synthesize *)
module mkTLPDispatcher(TLPDispatcher);
   FIFO#(TLPData#(16))  tlp_in_fifo     <- mkFIFO();
   Vector#(PortMax, FIFOF#(TLPData#(16))) tlp_out_fifo <- replicateM(mkGFIFOF(True,False)); // unguarded enq
   Reg#(Maybe#(Bit#(TLog#(PortMax)))) routeToPort <- mkReg(tagged Invalid);

   PulseWire is_read       <- mkPulseWire();
   PulseWire is_write      <- mkPulseWire();
   PulseWire is_completion <- mkPulseWire();

   function Bool configMatch(TLPData#(16) tlp);
      TLPMemoryIO3DWHeader hdr_3dw = unpack(tlp.data);
      Bool is_config_read    =  tlp.sof
                             && (tlp.hit == 7'h01)
                             && (hdr_3dw.format == MEM_READ_3DW_NO_DATA)
                             ;
      Bool is_config_write   =  tlp.sof
                             && (tlp.hit == 7'h01)
                             && (hdr_3dw.format == MEM_WRITE_3DW_DATA)
                             && (hdr_3dw.pkttype != COMPLETION)
                             ;
      return is_config_read || is_config_write;
   endfunction

   function Bool axiMatch(TLPData#(16) tlp);
      TLPMemoryIO3DWHeader hdr_3dw = unpack(tlp.data);
      Bool is_axi_read       =  tlp.sof
                             && (tlp.hit == 7'h04)
                             && (hdr_3dw.format == MEM_READ_3DW_NO_DATA)
                             ;
      Bool is_axi_write      =  tlp.sof
                             && (tlp.hit == 7'h04)
                             && (hdr_3dw.format == MEM_WRITE_3DW_DATA)
                             && (hdr_3dw.pkttype != COMPLETION)
                             ;
      return is_axi_read || is_axi_write;
   endfunction

   function Bool axiCompletionMatch(TLPData#(16) tlp);
      TLPMemoryIO3DWHeader hdr_3dw = unpack(tlp.data);
      Bool is_axi_completion =  tlp.sof
                             && (hdr_3dw.format == MEM_WRITE_3DW_DATA)
                             && (hdr_3dw.pkttype == COMPLETION)
                             ;
      return is_axi_completion;
   endfunction

   Vector#(PortMax, TlpMatchFunction) matchFunctions = newVector;
   matchFunctions[portConfig] = configMatch;
   matchFunctions[portPortal] = axiMatch;
   matchFunctions[portAxi]    = axiCompletionMatch;

   (* fire_when_enabled *)
   rule dispatch_incoming_TLP;
      TLPData#(16) tlp = tlp_in_fifo.first();
      TLPMemoryIO3DWHeader hdr_3dw = unpack(tlp.data);

      if (tlp.sof) begin
	 Bool matched = False;
         // route the packet based on this header
	 for (Integer port = 0; port < valueOf(PortMax); port = port+1)
            if (!matched && matchFunctions[port](tlp)) begin
	       matched = True;
               if (tlp_out_fifo[port].notFull()) begin
		  tlp_in_fifo.deq();
		  tlp_out_fifo[port].enq(tlp);
		  if (!tlp.eof)
                     routeToPort <= tagged Valid fromInteger(port);
               end
            end
         if (!matched) begin
            // unknown packet type -- just discard it
            tlp_in_fifo.deq();
         end
         // indicate activity type
         //if (is_config_read)                     is_read.send();
         //if (is_config_write)                    is_write.send();
      end
      else begin
	 if (routeToPort matches tagged Valid .port) begin
            if (tlp_out_fifo[port].notFull()) begin
               tlp_in_fifo.deq();
               tlp_out_fifo[port].enq(tlp);
               if (tlp.eof)
                  routeToPort <= tagged Invalid;
            end
         end
         else begin
            // unknown packet type -- just discard it
            tlp_in_fifo.deq();
         end
      end
   endrule: dispatch_incoming_TLP

   Vector#(PortMax, Get#(TLPData#(16))) outtemp;
   for (Integer i = 0; i < valueOf(PortMax); i=i+1)
       outtemp[i] = toGet(tlp_out_fifo[i]);
   interface out = outtemp;
   interface Put inFromBus    = toPut(tlp_in_fifo);
endmodule: mkTLPDispatcher

// Multiple sources of TLP packets must all share the PCIe bus. There
// is an arbiter which controls which source gets access to the PCIe
// endpoint.

interface TLPArbiter;
   // TLPs out to PCIe
   interface Get#(TLPData#(16)) outToBus;
   // TLPs in from the bridge implementation
   interface Vector#(PortMax, Put#(TLPData#(16))) in;
endinterface: TLPArbiter

(* synthesize *)
module mkTLPArbiter(TLPArbiter);
   FIFO#(TLPData#(16))  tlp_out_fifo     <- mkFIFO();
   Vector#(PortMax, FIFOF#(TLPData#(16))) tlp_in_fifo <- replicateM(mkGFIFOF(False,True)); // unguarded deq
   Vector#(PortMax, Reg#(Bool)) route_from <- replicateM(mkReg(False));

   PulseWire is_read       <- mkPulseWire();
   PulseWire is_write      <- mkPulseWire();
   PulseWire is_completion <- mkPulseWire();

   (* fire_when_enabled *)
   rule arbitrate_outgoing_TLP;
      if (route_from[portConfig]) begin
         // continue taking from the config FIFO until end-of-frame
         if (tlp_in_fifo[portConfig].notEmpty()) begin
            TLPData#(16) tlp = tlp_in_fifo[portConfig].first();
            tlp_in_fifo[portConfig].deq();
            tlp_out_fifo.enq(tlp);
            if (tlp.eof)
               route_from[portConfig] <= False;
         end
      end
      else if (route_from[portPortal]) begin
         // continue taking from the portal FIFO until end-of-frame
         if (tlp_in_fifo[portPortal].notEmpty()) begin
            TLPData#(16) tlp = tlp_in_fifo[portPortal].first();
            tlp_in_fifo[portPortal].deq();
            tlp_out_fifo.enq(tlp);
            if (tlp.eof)
               route_from[portPortal] <= False;
         end
      end
      else if (route_from[portAxi]) begin
         // continue taking from the axi FIFO until end-of-frame
         if (tlp_in_fifo[portAxi].notEmpty()) begin
            TLPData#(16) tlp = tlp_in_fifo[portAxi].first();
            tlp_in_fifo[portAxi].deq();
            tlp_out_fifo.enq(tlp);
            if (tlp.eof)
               route_from[portAxi] <= False;
         end
      end
      else if (tlp_in_fifo[portConfig].notEmpty()) begin
         // prioritize config read completions over portal traffic
         TLPData#(16) tlp = tlp_in_fifo[portConfig].first();
         tlp_in_fifo[portConfig].deq();
         if (tlp.sof) begin
            tlp_out_fifo.enq(tlp);
            if (!tlp.eof)
               route_from[portConfig] <= True;
            is_completion.send();
         end
      end
      else if (tlp_in_fifo[portPortal].notEmpty()) begin
         // prioritize portal read completions over AXI master traffic
         TLPData#(16) tlp = tlp_in_fifo[portPortal].first();
         tlp_in_fifo[portPortal].deq();
         if (tlp.sof) begin
            tlp_out_fifo.enq(tlp);
            if (!tlp.eof)
               route_from[portPortal] <= True;
            is_completion.send();
         end
      end
      else if (tlp_in_fifo[portAxi].notEmpty()) begin
         TLPData#(16) tlp = tlp_in_fifo[portAxi].first();
         tlp_in_fifo[portAxi].deq();
         if (tlp.sof) begin
            tlp_out_fifo.enq(tlp);
            if (!tlp.eof)
               route_from[portAxi] <= True;
            is_completion.send();
         end
      end
   endrule: arbitrate_outgoing_TLP

   Vector#(PortMax, Put#(TLPData#(16))) intemp;
   for (Integer i = 0; i < valueOf(PortMax); i=i+1)
       intemp[i] = toPut(tlp_in_fifo[i]);
   interface in = intemp;
   interface Get outToBus     = toGet(tlp_out_fifo);
endmodule
endpackage: PcieSplitter


define last:

define last:

// Copyright (c) 2013 Quanta Research Cambridge, Inc.

// Permission is hereby granted, free of charge, to any person
// obtaining a copy of this software and associated documentation
// files (the "Software"), to deal in the Software without
// restriction, including without limitation the rights to use, copy,
// modify, merge, publish, distribute, sublicense, and/or sell copies
// of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be
// included in all copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
// EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
// MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
// NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
// BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
// ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
// CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

import FIFO         :: *;
import FIFOF        :: *;
import GetPut       :: *;
import Connectable  :: *;
import PCIE         :: *;
import DefaultValue :: *;
import MIMO         :: *;
import MIFO         :: *;
import Vector       :: *;
import ClientServer :: *;
import MemTypes     :: *;

import AxiMasterSlave :: *;

interface MemSlaveEngine#(numeric type buswidth);
    interface Client#(TLPData#(16), TLPData#(16)) tlp;
    interface MemSlave#(40,buswidth) slave;
    method Bool tlpOutFifoNotEmpty();
    interface Reg#(Bool) use4dw;
endinterface: MemSlaveEngine

module mkMemSlaveEngine#(PciId my_id)(MemSlaveEngine#(buswidth))
   provisos (Div#(buswidth, 8, busWidthBytes),
	     Div#(buswidth, 32, busWidthWords),
	     Bits#(Vector#(busWidthWords, Bit#(32)), buswidth),
            Log#(busWidthBytes,beatShift),
	     Add#(aaa, 32, buswidth),
	     Add#(bbb, buswidth, 256),
	     Add#(ccc, TMul#(8, busWidthWords), 64),
	     Add#(ddd, TMul#(32, busWidthWords), 256),
	     Add#(eee, busWidthWords, 8),
	     Add#(1, a__, busWidthWords),
	     Add#(busWidthWords, b__, 4)
      );

    let beat_shift = fromInteger(valueOf(beatShift));

    FIFOF#(TLPData#(16)) tlpOutFifo <- mkFIFOF;
    FIFOF#(TLPData#(16)) tlpInFifo <- mkFIFOF;
    FIFO#(TLPData#(16)) tlpWriteHeaderFifo <- mkFIFO;
    FIFO#(TLPLength)      writeDwCountFifo <- mkFIFO;
    FIFO#(Bool)             writeIs3dwFifo <- mkFIFO;
    FIFO#(Bool)             writeIsHeaderOnlyFifo <- mkFIFO;

    Reg#(Bit#(7)) hitReg <- mkReg(0);
    Reg#(Bool) use4dwReg <- mkReg(True);

    // default configuration for MIMO is for guarded enq() and deq().
    // However, the implicit guard only checks for space for 1 element for enq(), and availability of 1 element for deq().
    MIMOConfiguration mimoCfg = defaultValue;
   MIFO#(4,busWidthWords,8,Bit#(32)) completionMimo <- mkMIFO();
   MIFO#(4,busWidthWords,8,TLPTag) completionTagMimo <- mkMIFO();

    MIMO#(busWidthWords,4,8,Bit#(32)) writeDataMimo <- mkMIMO(mimoCfg);
    Reg#(Bit#(9)) writeBurstCount <- mkReg(0);
    Reg#(TLPLength)  writeDwCount <- mkReg(0); // how many 4 byte (double) words to send
    Reg#(LUInt#(4))    tlpDwCount <- mkReg(0); // how many to send in the next tlp (at most 4)
    Reg#(Bool)            lastTlp <- mkReg(False); // if the next tlp sent is the last one
    Reg#(Bool)    writeInProgress <- mkReg(False);
    FIFOF#(TLPTag) writeTag <- mkSizedFIFOF(16);
    FIFOF#(TLPTag) doneTag <- mkSizedFIFOF(16);

    function LUInt#(4) tlpWordCount(TLPData#(16) tlp);
       if (tlp.be == 16'h0000)
	  return 0;
       else if (tlp.be == 16'h000f || tlp.be == 16'hf000)
	  return 1;
       else if (tlp.be == 16'h00ff || tlp.be == 16'hff00)
	  return 2;
       else if (tlp.be == 16'h0fff || tlp.be == 16'hfff0)
	  return 3;
       else if (tlp.be == 16'hffff)
	  return 4;
       else
	  return 0;
    endfunction

   rule writeHeaderTlp if (!writeInProgress && writeDataMimo.deqReadyN(1));
      let tlp <- toGet(tlpWriteHeaderFifo).get();
      let dwCount <- toGet(writeDwCountFifo).get();
      let is3dw <- toGet(writeIs3dwFifo).get();
      let isHeaderOnly <- toGet(writeIsHeaderOnlyFifo).get();

      TLPMemory4DWHeader hdr_4dw = unpack(tlp.data);

      TLPMemoryIO3DWHeader hdr_3dw = unpack(tlp.data);
      if (is3dw) begin
	 if (hdr_3dw.format != MEM_WRITE_3DW_DATA)
	    $display("MemSlaveEngine: expecting MEM_WRITE_3DW_DATA, got %d", hdr_3dw.format);
	 Vector#(4, Bit#(32)) v = writeDataMimo.first();
	 writeDataMimo.deq(1);
	 hdr_3dw.data = byteSwap(v[0]);
	 tlp.be = 16'hffff;
	 if (dwCount == 1)
	    tlp.eof = True;
	 dwCount = dwCount - 1;
	 tlp.data = pack(hdr_3dw);
      end
      else begin
	 tlp.be = 16'hffff;
      end

      tlpOutFifo.enq(tlp);
      $display("writeHeaderTlp dwCount=%d", dwCount);
      writeDwCount <= dwCount;
      tlpDwCount <= truncate(min(4,unpack(dwCount)));
      lastTlp <= (dwCount <= 4);
      writeInProgress <= (dwCount != 0);
      if (isHeaderOnly) begin
	 doneTag.enq(writeTag.first());
	 writeTag.deq();
      end

   endrule

   rule writeTlps if (writeInProgress && writeDataMimo.deqReadyN(tlpDwCount));
      TLPData#(16) tlp = defaultValue;
      tlp.sof = False;
      Vector#(4, Bit#(32)) v = unpack(0);

      // The MIMO implicit guard only checks for availability of 1 element
      // so we explicitly check for the number of elements required
      writeDataMimo.deq(tlpDwCount);
      v = writeDataMimo.first();
      let dwCount = writeDwCount - extend(pack(tlpDwCount));
      writeDwCount <= dwCount;
      tlpDwCount <= truncate(min(4,unpack(dwCount)));
      lastTlp <= (dwCount <= 4);
      if (tlpDwCount == 4)
	 tlp.be = 16'hffff;
      else if (tlpDwCount == 3)
	 tlp.be = 16'hfff0;
      else if (tlpDwCount == 2)
	 tlp.be = 16'hff00;
      else if (tlpDwCount == 1)
	 tlp.be = 16'hf000;
      tlp.eof = lastTlp;
      if (lastTlp) begin
	 writeInProgress <= False;
	 doneTag.enq(writeTag.first());
	 writeTag.deq();
	 $display("writeDwCount=%d will be zero", writeDwCount);
      end

      for (Integer i = 0; i < 4; i = i + 1)
	 tlp.data[(i+1)*32-1:i*32] = byteSwap(v[3-i]);
      tlpOutFifo.enq(tlp);
   endrule

   Reg#(TLPTag) lastTag <- mkReg(0);
   FIFOF#(TLPData#(16)) tlpDecodeFifo <- mkFIFOF();
   FIFOF#(LUInt#(4))    tlpWordCountFifo <- mkFIFOF();
   rule tlpInRule;
      let tlp <- toGet(tlpInFifo).get();
      tlpWordCountFifo.enq(tlpWordCount(tlp));
      tlpDecodeFifo.enq(tlp);
   endrule

   rule handleTlpRule;
      let tlp = tlpDecodeFifo.first;
      let count = tlpWordCountFifo.first;
      Bool handled = False;
      TLPMemoryIO3DWHeader h = unpack(tlp.data);
      hitReg <= tlp.hit;
      TLPMemoryIO3DWHeader hdr_3dw = unpack(tlp.data);
      TLPCompletionHeader hdr_completion = unpack(tlp.data);
      Vector#(4, Bit#(32)) vec = unpack(0);
      Vector#(4, Bit#(32)) tlpvec = unpack(tlp.data);

      if (!tlp.sof) begin
	 vec = reverse(tlpvec);
	 // The MIMO implicit guard only checks for space to enqueue 1 element
	 // so we explicitly check for the number of elements required
	 // otherwise elements in the queue will be overwritten.
	 if (completionMimo.enqReady()
	    && completionTagMimo.enqReady())
	    begin
	       completionMimo.enq(count, vec);
	       Vector#(4, TLPTag) tagvec = replicate(lastTag);
	       completionTagMimo.enq(count, tagvec);
	       handled = True;
	    end
      end
      else if (hdr_3dw.format == MEM_WRITE_3DW_DATA
	       && hdr_3dw.pkttype == COMPLETION
	       && completionMimo.enqReady()
	       && completionTagMimo.enqReady()) begin
	    vec[0] = hdr_3dw.data;
	    completionMimo.enq(1, vec);
            TLPTag tag = hdr_completion.tag;
	    lastTag <= tag;
	    completionTagMimo.enq(1, replicate(tag));
	    handled = True;
      end
      //$display("tlpIn handled=%d tlp=%h\n", handled, tlp);
      if (handled) begin
	 tlpDecodeFifo.deq();
	 tlpWordCountFifo.deq();
      end
   endrule

   FIFO#(MemRequest#(40)) readReqFifo <- mkFIFO();
   rule readReqRule if (!writeInProgress && !writeDataMimo.deqReady());
      let req <- toGet(readReqFifo).get();
      let burstLen = req.burstLen >> beat_shift;
      let addr = req.addr;
      let arid = req.tag;

      TLPData#(16) tlp = defaultValue;
      tlp.sof = True;
      tlp.eof = True;
      tlp.hit = 7'h00;
      TLPLength tlplen = fromInteger(valueOf(busWidthWords))*(extend(burstLen));
      if (addr[39:32] != 0) begin
	 TLPMemory4DWHeader hdr_4dw = defaultValue;
	 hdr_4dw.format = MEM_READ_4DW_NO_DATA;
	 hdr_4dw.tag = extend(arid);
	 hdr_4dw.reqid = my_id;
	 hdr_4dw.nosnoop = SNOOPING_REQD;
	 hdr_4dw.addr = addr[40-1:2];
	 hdr_4dw.length = tlplen;
	 hdr_4dw.firstbe = 4'hf;
	 hdr_4dw.lastbe = (tlplen > 1) ? 4'hf : 0;
	 tlp.data = pack(hdr_4dw);
	 tlp.be = 16'hffff;
      end
      else begin
	 TLPMemoryIO3DWHeader hdr_3dw = defaultValue;
	 hdr_3dw.format = MEM_READ_3DW_NO_DATA;
	 hdr_3dw.tag = extend(arid);
	 hdr_3dw.reqid = my_id;
	 hdr_3dw.nosnoop = SNOOPING_REQD;
	 hdr_3dw.addr = addr[32-1:2];
	 hdr_3dw.length = tlplen;
	 hdr_3dw.firstbe = 4'hf;
	 hdr_3dw.lastbe = (tlplen > 1) ? 4'hf : 0;
	 tlp.data = pack(hdr_3dw);
	 tlp.be = 16'hfff0;
      end
      tlpOutFifo.enq(tlp);
   endrule

    interface Client        tlp;
        interface request = toGet(tlpOutFifo);
        interface response = toPut(tlpInFifo);
    endinterface
    interface MemSlave slave;
   interface MemWriteServer write_server;
      interface Put writeReq;
         method Action put(MemRequest#(40) req) if (writeBurstCount == 0);
	    let burstLen = req.burstLen >> beat_shift;
	    let addr = req.addr;
	    let awid = req.tag;
	    let writeIs3dw = False;

	    TLPLength tlplen = fromInteger(valueOf(busWidthWords))*(extend(burstLen));
	    TLPData#(16) tlp = defaultValue;
	    tlp.sof = True;
	    tlp.eof = False;
	    tlp.hit = 7'h00;
	    tlp.be = 16'hffff;

	    $display("slave.writeAddr tlplen=%d burstLen=%d", tlplen, burstLen);
	    if ((addr >> 32) != 0) begin
	       TLPMemory4DWHeader hdr_4dw = defaultValue;
	       hdr_4dw.format = MEM_WRITE_4DW_DATA;
	       hdr_4dw.tag = extend(awid);
	       hdr_4dw.reqid = my_id;
	       hdr_4dw.nosnoop = SNOOPING_REQD;
	       hdr_4dw.addr = addr[40-1:2];
	       hdr_4dw.length = tlplen;
	       hdr_4dw.firstbe = 4'hf;
	       hdr_4dw.lastbe = (tlplen > 1) ? 4'hf : 0;
	       tlp.data = pack(hdr_4dw);
	    end
	    else begin
	       writeIs3dw = True;
	       TLPMemoryIO3DWHeader hdr_3dw = defaultValue;
	       hdr_3dw.format = MEM_WRITE_3DW_DATA;
	       hdr_3dw.tag = extend(awid);
	       hdr_3dw.reqid = my_id;
	       hdr_3dw.nosnoop = SNOOPING_REQD;
	       hdr_3dw.addr = addr[32-1:2];
	       hdr_3dw.length = tlplen;
	       hdr_3dw.firstbe = 4'hf;
	       hdr_3dw.lastbe = (tlplen > 1) ? 4'hf : 0;

	       tlp.be = 16'hfff0; // no data word in this TLP

	       tlp.data = pack(hdr_3dw);
	    end
	    tlpWriteHeaderFifo.enq(tlp);
	    writeDwCountFifo.enq(tlplen);
	    writeIs3dwFifo.enq(writeIs3dw);
	    writeIsHeaderOnlyFifo.enq(writeIs3dw && tlplen == 1);
	    writeBurstCount <= zeroExtend(burstLen);
	    writeTag.enq(extend(awid));
         endmethod
	endinterface
      interface Put writeData;
         method Action put(MemData#(buswidth) wdata)
	      provisos (Bits#(Vector#(busWidthWords, Bit#(32)), busWidth)) if (writeBurstCount > 0 && writeDataMimo.enqReadyN(fromInteger(valueOf(busWidthWords))));

	      writeBurstCount <= writeBurstCount - 1;
	      Vector#(busWidthWords, Bit#(32)) v = unpack(wdata.data);
	      writeDataMimo.enq(fromInteger(valueOf(busWidthWords)), v);
           endmethod
       endinterface
      interface Get writeDone;
         method ActionValue#(Bit#(ObjectTagSize)) get();
	      let tag = doneTag.first();
	      doneTag.deq();
	      return truncate(tag);
           endmethod
	endinterface
   endinterface
   interface MemReadServer read_server;
      interface Put readReq;
         method Action put(MemRequest#(40) req);
	    readReqFifo.enq(req);
         endmethod
       endinterface
      interface Get     readData;
         method ActionValue#(MemData#(buswidth)) get() if (completionMimo.deqReady()
							   && completionTagMimo.deqReady());
	      let data_v = completionMimo.first;
	      let tag_v = completionTagMimo.first;
	      completionMimo.deq();
	      completionTagMimo.deq();
              Bit#(buswidth) v = 0;
	      for (Integer i = 0; i < valueOf(busWidthWords); i = i+1)
		 v[(i+1)*32-1:i*32] = byteSwap(data_v[i]);
	      return MemData { data: v, tag: truncate(tag_v[0]), last: True};
           endmethod
	endinterface
   endinterface
    endinterface: slave
   method Bool tlpOutFifoNotEmpty() = tlpOutFifo.notEmpty;
   interface Reg use4dw = use4dwReg;
endmodule: mkMemSlaveEngine

// Copyright (c) 2013 Quanta Research Cambridge, Inc.

// Permission is hereby granted, free of charge, to any person
// obtaining a copy of this software and associated documentation
// files (the "Software"), to deal in the Software without
// restriction, including without limitation the rights to use, copy,
// modify, merge, publish, distribute, sublicense, and/or sell copies
// of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be
// included in all copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
// EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
// MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
// NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
// BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
// ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
// CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

import FIFO         :: *;
import FIFOF        :: *;
import GetPut       :: *;
import Connectable  :: *;
import PCIE         :: *;
import DefaultValue :: *;
import MIMO         :: *;
import MIFO         :: *;
import Vector       :: *;
import ClientServer :: *;
import MemTypes     :: *;

import AxiMasterSlave :: *;

interface MemSlaveEngine#(numeric type buswidth);
    interface Client#(TLPData#(16), TLPData#(16)) tlp;
    interface MemSlave#(40,buswidth) slave;
    method Bool tlpOutFifoNotEmpty();
    interface Reg#(Bool) use4dw;
endinterface: MemSlaveEngine

module mkMemSlaveEngine#(PciId my_id)(MemSlaveEngine#(buswidth))
   provisos (Div#(buswidth, 8, busWidthBytes),
	     Div#(buswidth, 32, busWidthWords),
	     Bits#(Vector#(busWidthWords, Bit#(32)), buswidth),
            Log#(busWidthBytes,beatShift),
	     Add#(aaa, 32, buswidth),
	     Add#(bbb, buswidth, 256),
	     Add#(ccc, TMul#(8, busWidthWords), 64),
	     Add#(ddd, TMul#(32, busWidthWords), 256),
	     Add#(eee, busWidthWords, 8),
	     Add#(1, a__, busWidthWords),
	     Add#(busWidthWords, b__, 4)
      );

    let beat_shift = fromInteger(valueOf(beatShift));

    FIFOF#(TLPData#(16)) tlpOutFifo <- mkFIFOF;
    FIFOF#(TLPData#(16)) tlpInFifo <- mkFIFOF;
    FIFO#(TLPData#(16)) tlpWriteHeaderFifo <- mkFIFO;
    FIFO#(TLPLength)      writeDwCountFifo <- mkFIFO;
    FIFO#(Bool)             writeIs3dwFifo <- mkFIFO;
    FIFO#(Bool)             writeIsHeaderOnlyFifo <- mkFIFO;

    Reg#(Bit#(7)) hitReg <- mkReg(0);
    Reg#(Bool) use4dwReg <- mkReg(True);

    // default configuration for MIMO is for guarded enq() and deq().
    // However, the implicit guard only checks for space for 1 element for enq(), and availability of 1 element for deq().
    MIMOConfiguration mimoCfg = defaultValue;
   MIFO#(4,busWidthWords,8,Bit#(32)) completionMimo <- mkMIFO();
   MIFO#(4,busWidthWords,8,TLPTag) completionTagMimo <- mkMIFO();

    MIMO#(busWidthWords,4,8,Bit#(32)) writeDataMimo <- mkMIMO(mimoCfg);
    Reg#(Bit#(9)) writeBurstCount <- mkReg(0);
    Reg#(TLPLength)  writeDwCount <- mkReg(0); // how many 4 byte (double) words to send
    Reg#(LUInt#(4))    tlpDwCount <- mkReg(0); // how many to send in the next tlp (at most 4)
    Reg#(Bool)            lastTlp <- mkReg(False); // if the next tlp sent is the last one
    Reg#(Bool)    writeInProgress <- mkReg(False);
    FIFOF#(TLPTag) writeTag <- mkSizedFIFOF(16);
    FIFOF#(TLPTag) doneTag <- mkSizedFIFOF(16);

    function LUInt#(4) tlpWordCount(TLPData#(16) tlp);
       if (tlp.be == 16'h0000)
	  return 0;
       else if (tlp.be == 16'h000f || tlp.be == 16'hf000)
	  return 1;
       else if (tlp.be == 16'h00ff || tlp.be == 16'hff00)
	  return 2;
       else if (tlp.be == 16'h0fff || tlp.be == 16'hfff0)
	  return 3;
       else if (tlp.be == 16'hffff)
	  return 4;
       else
	  return 0;
    endfunction

   rule writeHeaderTlp if (!writeInProgress && writeDataMimo.deqReadyN(1));
      let tlp <- toGet(tlpWriteHeaderFifo).get();
      let dwCount <- toGet(writeDwCountFifo).get();
      let is3dw <- toGet(writeIs3dwFifo).get();
      let isHeaderOnly <- toGet(writeIsHeaderOnlyFifo).get();

      TLPMemory4DWHeader hdr_4dw = unpack(tlp.data);

      TLPMemoryIO3DWHeader hdr_3dw = unpack(tlp.data);
      if (is3dw) begin
	 if (hdr_3dw.format != MEM_WRITE_3DW_DATA)
	    $display("MemSlaveEngine: expecting MEM_WRITE_3DW_DATA, got %d", hdr_3dw.format);
	 Vector#(4, Bit#(32)) v = writeDataMimo.first();
	 writeDataMimo.deq(1);
	 hdr_3dw.data = byteSwap(v[0]);
	 tlp.be = 16'hffff;
	 if (dwCount == 1)
	    tlp.eof = True;
	 dwCount = dwCount - 1;
	 tlp.data = pack(hdr_3dw);
      end
      else begin
	 tlp.be = 16'hffff;
      end

      tlpOutFifo.enq(tlp);
      $display("writeHeaderTlp dwCount=%d", dwCount);
      writeDwCount <= dwCount;
      tlpDwCount <= truncate(min(4,unpack(dwCount)));
      lastTlp <= (dwCount <= 4);
      writeInProgress <= (dwCount != 0);
      if (isHeaderOnly) begin
	 doneTag.enq(writeTag.first());
	 writeTag.deq();
      end

   endrule

   rule writeTlps if (writeInProgress && writeDataMimo.deqReadyN(tlpDwCount));
      TLPData#(16) tlp = defaultValue;
      tlp.sof = False;
      Vector#(4, Bit#(32)) v = unpack(0);

      // The MIMO implicit guard only checks for availability of 1 element
      // so we explicitly check for the number of elements required
      writeDataMimo.deq(tlpDwCount);
      v = writeDataMimo.first();
      let dwCount = writeDwCount - extend(pack(tlpDwCount));
      writeDwCount <= dwCount;
      tlpDwCount <= truncate(min(4,unpack(dwCount)));
      lastTlp <= (dwCount <= 4);
      if (tlpDwCount == 4)
	 tlp.be = 16'hffff;
      else if (tlpDwCount == 3)
	 tlp.be = 16'hfff0;
      else if (tlpDwCount == 2)
	 tlp.be = 16'hff00;
      else if (tlpDwCount == 1)
	 tlp.be = 16'hf000;
      tlp.eof = lastTlp;
      if (lastTlp) begin
	 writeInProgress <= False;
	 doneTag.enq(writeTag.first());
	 writeTag.deq();
	 $display("writeDwCount=%d will be zero", writeDwCount);
      end

      for (Integer i = 0; i < 4; i = i + 1)
	 tlp.data[(i+1)*32-1:i*32] = byteSwap(v[3-i]);
      tlpOutFifo.enq(tlp);
   endrule

   Reg#(TLPTag) lastTag <- mkReg(0);
   FIFOF#(TLPData#(16)) tlpDecodeFifo <- mkFIFOF();
   FIFOF#(LUInt#(4))    tlpWordCountFifo <- mkFIFOF();
   rule tlpInRule;
      let tlp <- toGet(tlpInFifo).get();
      tlpWordCountFifo.enq(tlpWordCount(tlp));
      tlpDecodeFifo.enq(tlp);
   endrule

   rule handleTlpRule;
      let tlp = tlpDecodeFifo.first;
      let count = tlpWordCountFifo.first;
      Bool handled = False;
      TLPMemoryIO3DWHeader h = unpack(tlp.data);
      hitReg <= tlp.hit;
      TLPMemoryIO3DWHeader hdr_3dw = unpack(tlp.data);
      TLPCompletionHeader hdr_completion = unpack(tlp.data);
      Vector#(4, Bit#(32)) vec = unpack(0);
      Vector#(4, Bit#(32)) tlpvec = unpack(tlp.data);

      if (!tlp.sof) begin
	 vec = reverse(tlpvec);
	 // The MIMO implicit guard only checks for space to enqueue 1 element
	 // so we explicitly check for the number of elements required
	 // otherwise elements in the queue will be overwritten.
	 if (completionMimo.enqReady()
	    && completionTagMimo.enqReady())
	    begin
	       completionMimo.enq(count, vec);
	       Vector#(4, TLPTag) tagvec = replicate(lastTag);
	       completionTagMimo.enq(count, tagvec);
	       handled = True;
	    end
      end
      else if (hdr_3dw.format == MEM_WRITE_3DW_DATA
	       && hdr_3dw.pkttype == COMPLETION
	       && completionMimo.enqReady()
	       && completionTagMimo.enqReady()) begin
	    vec[0] = hdr_3dw.data;
	    completionMimo.enq(1, vec);
            TLPTag tag = hdr_completion.tag;
	    lastTag <= tag;
	    completionTagMimo.enq(1, replicate(tag));
	    handled = True;
      end
      //$display("tlpIn handled=%d tlp=%h\n", handled, tlp);
      if (handled) begin
	 tlpDecodeFifo.deq();
	 tlpWordCountFifo.deq();
      end
   endrule

   FIFO#(MemRequest#(40)) readReqFifo <- mkFIFO();
   rule readReqRule if (!writeInProgress && !writeDataMimo.deqReady());
      let req <- toGet(readReqFifo).get();
      let burstLen = req.burstLen >> beat_shift;
      let addr = req.addr;
      let arid = req.tag;

      TLPData#(16) tlp = defaultValue;
      tlp.sof = True;
      tlp.eof = True;
      tlp.hit = 7'h00;
      TLPLength tlplen = fromInteger(valueOf(busWidthWords))*(extend(burstLen));
      if (addr[39:32] != 0) begin
	 TLPMemory4DWHeader hdr_4dw = defaultValue;
	 hdr_4dw.format = MEM_READ_4DW_NO_DATA;
	 hdr_4dw.tag = extend(arid);
	 hdr_4dw.reqid = my_id;
	 hdr_4dw.nosnoop = SNOOPING_REQD;
	 hdr_4dw.addr = addr[40-1:2];
	 hdr_4dw.length = tlplen;
	 hdr_4dw.firstbe = 4'hf;
	 hdr_4dw.lastbe = (tlplen > 1) ? 4'hf : 0;
	 tlp.data = pack(hdr_4dw);
	 tlp.be = 16'hffff;
      end
      else begin
	 TLPMemoryIO3DWHeader hdr_3dw = defaultValue;
	 hdr_3dw.format = MEM_READ_3DW_NO_DATA;
	 hdr_3dw.tag = extend(arid);
	 hdr_3dw.reqid = my_id;
	 hdr_3dw.nosnoop = SNOOPING_REQD;
	 hdr_3dw.addr = addr[32-1:2];
	 hdr_3dw.length = tlplen;
	 hdr_3dw.firstbe = 4'hf;
	 hdr_3dw.lastbe = (tlplen > 1) ? 4'hf : 0;
	 tlp.data = pack(hdr_3dw);
	 tlp.be = 16'hfff0;
      end
      tlpOutFifo.enq(tlp);
   endrule

    interface Client        tlp;
        interface request = toGet(tlpOutFifo);
        interface response = toPut(tlpInFifo);
    endinterface
    interface MemSlave slave;
   interface MemWriteServer write_server;
      interface Put writeReq;
         method Action put(MemRequest#(40) req) if (writeBurstCount == 0);
	    let burstLen = req.burstLen >> beat_shift;
	    let addr = req.addr;
	    let awid = req.tag;
	    let writeIs3dw = False;

	    TLPLength tlplen = fromInteger(valueOf(busWidthWords))*(extend(burstLen));
	    TLPData#(16) tlp = defaultValue;
	    tlp.sof = True;
	    tlp.eof = False;
	    tlp.hit = 7'h00;
	    tlp.be = 16'hffff;

	    $display("slave.writeAddr tlplen=%d burstLen=%d", tlplen, burstLen);
	    if ((addr >> 32) != 0) begin
	       TLPMemory4DWHeader hdr_4dw = defaultValue;
	       hdr_4dw.format = MEM_WRITE_4DW_DATA;
	       hdr_4dw.tag = extend(awid);
	       hdr_4dw.reqid = my_id;
	       hdr_4dw.nosnoop = SNOOPING_REQD;
	       hdr_4dw.addr = addr[40-1:2];
	       hdr_4dw.length = tlplen;
	       hdr_4dw.firstbe = 4'hf;
	       hdr_4dw.lastbe = (tlplen > 1) ? 4'hf : 0;
	       tlp.data = pack(hdr_4dw);
	    end
	    else begin
	       writeIs3dw = True;
	       TLPMemoryIO3DWHeader hdr_3dw = defaultValue;
	       hdr_3dw.format = MEM_WRITE_3DW_DATA;
	       hdr_3dw.tag = extend(awid);
	       hdr_3dw.reqid = my_id;
	       hdr_3dw.nosnoop = SNOOPING_REQD;
	       hdr_3dw.addr = addr[32-1:2];
	       hdr_3dw.length = tlplen;
	       hdr_3dw.firstbe = 4'hf;
	       hdr_3dw.lastbe = (tlplen > 1) ? 4'hf : 0;

	       tlp.be = 16'hfff0; // no data word in this TLP

	       tlp.data = pack(hdr_3dw);
	    end
	    tlpWriteHeaderFifo.enq(tlp);
	    writeDwCountFifo.enq(tlplen);
	    writeIs3dwFifo.enq(writeIs3dw);
	    writeIsHeaderOnlyFifo.enq(writeIs3dw && tlplen == 1);
	    writeBurstCount <= zeroExtend(burstLen);
	    writeTag.enq(extend(awid));
         endmethod
	endinterface
      interface Put writeData;
         method Action put(MemData#(buswidth) wdata)
	      provisos (Bits#(Vector#(busWidthWords, Bit#(32)), busWidth)) if (writeBurstCount > 0 && writeDataMimo.enqReadyN(fromInteger(valueOf(busWidthWords))));

	      writeBurstCount <= writeBurstCount - 1;
	      Vector#(busWidthWords, Bit#(32)) v = unpack(wdata.data);
	      writeDataMimo.enq(fromInteger(valueOf(busWidthWords)), v);
           endmethod
       endinterface
      interface Get writeDone;
         method ActionValue#(Bit#(ObjectTagSize)) get();
	      let tag = doneTag.first();
	      doneTag.deq();
	      return truncate(tag);
           endmethod
	endinterface
   endinterface
   interface MemReadServer read_server;
      interface Put readReq;
         method Action put(MemRequest#(40) req);
	    readReqFifo.enq(req);
         endmethod
       endinterface
      interface Get     readData;
         method ActionValue#(MemData#(buswidth)) get() if (completionMimo.deqReady()
							   && completionTagMimo.deqReady());
	      let data_v = completionMimo.first;
	      let tag_v = completionTagMimo.first;
	      completionMimo.deq();
	      completionTagMimo.deq();
              Bit#(buswidth) v = 0;
	      for (Integer i = 0; i < valueOf(busWidthWords); i = i+1)
		 v[(i+1)*32-1:i*32] = byteSwap(data_v[i]);
	      return MemData { data: v, tag: truncate(tag_v[0])};
           endmethod
	endinterface
   endinterface
    endinterface: slave
   method Bool tlpOutFifoNotEmpty() = tlpOutFifo.notEmpty;
   interface Reg use4dw = use4dwReg;
endmodule: mkMemSlaveEngine


fixed log_read_cnt timestamp to timeof(receive), not timeof(read)

// Copyright (c) 2015 Cornell University.

// Permission is hereby granted, free of charge, to any person
// obtaining a copy of this software and associated documentation
// files (the "Software"), to deal in the Software without
// restriction, including without limitation the rights to use, copy,
// modify, merge, publish, distribute, sublicense, and/or sell copies
// of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be
// included in all copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
// EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
// MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
// NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
// BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
// ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
// CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

import Clocks::*;
import FIFO::*;
import FIFOF::*;
import Vector::*;
import Pipe::*;
import GetPut::*;
import Probe::*;

typedef struct {
   Bit#(8) port_no;
   Bit#(64) data;
} BufData deriving (Bits, Eq);

interface DtpIfc;
   interface PipeIn#(Bit#(128)) timestamp; // streaming time counter from NetTop.
   interface Vector#(4, PipeOut#(Bit#(53))) fromHost;
   interface Vector#(4, PipeIn#(Bit#(53)))  toHost;
   interface Vector#(4, PipeIn#(Bit#(32)))  delay;
   interface Vector#(4, PipeIn#(Bit#(32)))  state;
   interface Vector#(4, PipeIn#(Bit#(64)))  jumpCount;
   interface Vector#(4, PipeIn#(Bit#(53)))  cLocal;
   interface PipeIn#(Bit#(53)) globalOut;
   interface Vector#(4, PipeOut#(Bit#(32))) interval;
   interface Reset rst;
   interface PipeIn#(Bit#(1)) isSwitch;
endinterface

interface SonicUserRequest;
   method Action dtp_read_version();
   method Action dtp_reset(Bit#(32) len);
   method Action dtp_set_cnt(Bit#(8) port_no, Bit#(64) c);
   method Action dtp_read_delay(Bit#(8) port_no);
   method Action dtp_read_state(Bit#(8) port_no);
   method Action dtp_read_error(Bit#(8) port_no);
   method Action dtp_read_cnt(Bit#(8) cmd);
   method Action dtp_logger_write_cnt(Bit#(8) port_no, Bit#(64) local_cnt);
   method Action dtp_logger_read_cnt(Bit#(8) port_no);
   method Action dtp_read_local_cnt(Bit#(8) port_no);
   method Action dtp_read_global_cnt();
   method Action dtp_set_beacon_interval(Bit#(8) port_no, Bit#(32) interval);
   method Action dtp_read_beacon_interval(Bit#(8) port_no);
/* NOT IMPLEMENTED YET.
   method Action dtp_ctrl_disable();
   method Action dtp_ctrl_enable();
*/
endinterface

interface SonicUserIndication;
   method Action dtp_read_version_resp(Bit#(32) version);
   method Action dtp_read_delay_resp(Bit#(8) port_no, Bit#(32) delay);
   method Action dtp_read_state_resp(Bit#(8) port_no, Bit#(32) state);
   method Action dtp_read_error_resp(Bit#(8) port_no, Bit#(64) jumpc);
   method Action dtp_read_cnt_resp(Bit#(64) val);
   method Action dtp_logger_read_cnt_resp(Bit#(8) port_no, Bit#(64) localc, Bit#(64) msg1, Bit#(64) msg2);
   method Action dtp_read_local_cnt_resp(Bit#(8) port_no, Bit#(64) val);
   method Action dtp_read_global_cnt_resp(Bit#(64) val);
   method Action dtp_read_beacon_interval_resp(Bit#(8) port_no, Bit#(32) interval);
endinterface

interface SonicUser;
   interface SonicUserRequest request;
   interface DtpIfc           dtp;
endinterface

module mkSonicUser#(SonicUserIndication indication)(SonicUser);
   let verbose = False;
   Clock defaultClock <- exposeCurrentClock();

   Reg#(Bit#(64))  cycle_count <- mkReg(0);
   Reg#(Bit#(64))  last_count  <- mkReg(0);

   FIFOF#(Bit#(128)) cntFifo <- mkFIFOF();
   Reg#(Bit#(128))   cycle_reg <- mkReg(0);
   Reg#(Bit#(1))     isSwitch_reg <- mkReg(0);
   Vector#(4, Reg#(Bit#(32))) beacon_interval <- replicateM(mkReg(1000)); //default beacon interval is 1000.

   Vector#(4, FIFOF#(Bit#(53))) fromHostFifo <- replicateM(mkSizedFIFOF(4));
   Vector#(4, FIFOF#(Bit#(53))) toHostFifo   <- replicateM(mkSizedFIFOF(4));
   Vector#(4, FIFOF#(Bit#(32))) delayFifo    <- replicateM(mkSizedFIFOF(4));
   Vector#(4, FIFOF#(Bit#(32))) stateFifo    <- replicateM(mkSizedFIFOF(4));
   Vector#(4, FIFOF#(Bit#(64))) jumpCountFifo <- replicateM(mkSizedFIFOF(4));
   Vector#(4, FIFOF#(Bit#(53))) cLocalFifo   <- replicateM(mkSizedFIFOF(4));
   FIFOF#(Bit#(53)) cGlobalFifo <- mkSizedFIFOF(4);
   Vector#(4, FIFOF#(Bit#(32))) intervalFifo <- replicateM(mkSizedFIFOF(4));
   FIFOF#(Bit#(1)) isSwitchFifo <- mkFIFOF();

   Reg#(Bit#(8))  lwrite_port <- mkReg(0);
   FIFOF#(BufData) lwrite_data_cycle1 <- mkSizedFIFOF(8);
   FIFOF#(BufData) lwrite_data_cycle2 <- mkSizedFIFOF(8);
   FIFOF#(void) log_write_cf <- mkFIFOF;

   Vector#(4, FIFOF#(BufData)) lread_data_cycle1 <- replicateM(mkSizedFIFOF(8));
   Vector#(4, FIFOF#(BufData)) lread_data_cycle2 <- replicateM(mkSizedFIFOF(8));
   Vector#(4, FIFOF#(Bit#(53))) lread_data_timestamp <- replicateM(mkSizedFIFOF(8));

   Reg#(Bit#(28)) dtp_rst_cntr <- mkReg(0);
   MakeResetIfc dtpResetOut <- mkResetSync(0, False, defaultClock);

   rule count;
      cycle_count <= cycle_count + 1;
   endrule

   // clear all fifo on reset
   rule clearOnReset(dtpResetOut.isAsserted);
      for (Integer i=0; i<4; i=i+1) begin
         fromHostFifo[i].clear();
         toHostFifo[i].clear();
         delayFifo[i].clear();
         stateFifo[i].clear();
         jumpCountFifo[i].clear();
         cLocalFifo[i].clear();
         intervalFifo[i].clear();
         lread_data_cycle1[i].clear();
         lread_data_cycle2[i].clear();
      end
      cGlobalFifo.clear();
      cntFifo.clear();
      lwrite_data_cycle1.clear();
      lwrite_data_cycle2.clear();
      log_write_cf.clear();
   endrule

   // dtp_read_cnt
   rule snapshot_dtp_timestamp;
      let v <- toGet(cntFifo).get;
      cycle_reg <= v;
   endrule

   // dtp_read_delay
   Vector#(4, Reg#(Bit#(32))) delay_reg <- replicateM(mkReg(0));
   for (Integer i=0; i<4; i=i+1) begin
      rule snapshot_delay;
         let v <- toGet(delayFifo[i]).get;
         delay_reg[i] <= v;
      endrule
   end

   // dtp_read_state
   Vector#(4, Reg#(Bit#(32))) state_reg <- replicateM(mkReg(0));
   for (Integer i=0; i<4; i=i+1) begin
      rule snapshot_state;
         let v <- toGet(stateFifo[i]).get;
         state_reg[i] <= v;
      endrule
   end

   // dtp_read_error
   Vector#(4, Reg#(Bit#(64))) jumpc_reg <- replicateM(mkReg(0));
   for (Integer i=0; i<4; i=i+1) begin
      rule snapshot_jumpc;
         let v <- toGet(jumpCountFifo[i]).get;
         jumpc_reg[i] <= v;
      endrule
   end

   // dtp_read_local_cnt
   Vector#(4, Reg#(Bit#(53))) clocal_reg <- replicateM(mkReg(0));
   for (Integer i=0; i<4; i=i+1) begin
      rule snapshot_clocal;
         let v <- toGet(cLocalFifo[i]).get;
         clocal_reg[i] <= v;
      endrule
   end

   // dtp_read_global_cnt
   Reg#(Bit#(53)) cglobal_reg <- mkReg(0);
   rule snapshot_cglobal;
      let v <- toGet(cGlobalFifo).get;
      cglobal_reg <= v;
   endrule

   // dtp_set_interval
   for (Integer i=0; i<4; i=i+1) begin
      rule set_interval;
         let v = beacon_interval[i];
         intervalFifo[i].enq(v);
      endrule
   end

   // dtp_logger_write_cnt
   rule log_from_host_cycle1;
      let v = lwrite_data_cycle1.first;
      if (fromHostFifo[v.port_no].notFull) begin
         fromHostFifo[v.port_no].enq({1'b0, truncate(v.data)});
         log_write_cf.enq(?);
         lwrite_data_cycle1.deq;
      end
   endrule
   rule log_from_host_cycle2 (log_write_cf.notEmpty);
      let v = lwrite_data_cycle2.first;
      if (fromHostFifo[v.port_no].notFull) begin
         fromHostFifo[v.port_no].enq({1'b1, truncate(v.data)});
         lwrite_data_cycle2.deq;
         log_write_cf.deq;
      end
   endrule

   for (Integer i=0; i<4; i=i+1) begin
      rule save_host_data (toHostFifo[i].notEmpty && lread_data_cycle1[i].notFull &&
                           lread_data_cycle2[i].notFull && lread_data_timestamp[i].notFull);
         Bit#(53) v = toHostFifo[i].first;
         case (v[52]) matches
            0: begin
               lread_data_cycle1[i].enq(BufData{port_no:fromInteger(i), data:zeroExtend(v[51:0])});
            end
            1: begin
               lread_data_cycle2[i].enq(BufData{port_no:fromInteger(i), data:zeroExtend(v[51:0])});
               Bit#(53) timestamp = 0;
               if (isSwitch_reg == 0) begin // 0 for NIC, 1 for Switch
                  timestamp = clocal_reg[i];
               end
               else begin
                  timestamp = cglobal_reg;
               end
               lread_data_timestamp[i].enq(timestamp);
            end
         endcase
         toHostFifo[i].deq;
      endrule
   end

   rule assert_reset (dtp_rst_cntr != 0);
      dtp_rst_cntr <= dtp_rst_cntr - 1;
      dtpResetOut.assertReset;
   endrule

   rule switch_mode;
      let v <- toGet(isSwitchFifo).get;
      isSwitch_reg <= v;
   endrule

   // Interface to external modules.
   interface dtp = (interface DtpIfc;
      interface timestamp = toPipeIn(cntFifo);
      interface delay     = map(toPipeIn, delayFifo);
      interface state     = map(toPipeIn, stateFifo);
      interface jumpCount = map(toPipeIn, jumpCountFifo);
      interface toHost    = map(toPipeIn, toHostFifo);
      interface fromHost  = map(toPipeOut, fromHostFifo);
      interface cLocal    = map(toPipeIn, cLocalFifo);
      interface globalOut = toPipeIn(cGlobalFifo);
      interface interval  = map(toPipeOut, intervalFifo);
      interface rst       = dtpResetOut.new_rst;
      interface isSwitch  = toPipeIn(isSwitchFifo);
   endinterface);

   // API implementation
   interface SonicUserRequest request;
   method Action dtp_read_version();
      let v = `DtpVersion; //Defined in Makefile as time of compilation.
      indication.dtp_read_version_resp(v);
   endmethod
   method Action dtp_reset(Bit#(32) len);
      Bit#(28) limit = truncate(len) & 28'hFFFFFFF;
      if (limit == 0) limit = 32;
      dtp_rst_cntr <= limit;
   endmethod
   method Action dtp_set_cnt(Bit#(8) port_no, Bit#(64) c);
      //
   endmethod
   method Action dtp_read_delay(Bit#(8) port_no);
      if (port_no < 4) begin
         indication.dtp_read_delay_resp(port_no, truncate(delay_reg[port_no]));
      end
   endmethod
   method Action dtp_read_state(Bit#(8) port_no);
      if (port_no < 4) begin
         indication.dtp_read_state_resp(port_no, state_reg[port_no]);
      end
   endmethod
   method Action dtp_read_error(Bit#(8) port_no);
      if (port_no < 4) begin
         indication.dtp_read_error_resp(port_no, jumpc_reg[port_no]);
      end
   endmethod
   method Action dtp_read_cnt(Bit#(8) port_no);
      indication.dtp_read_cnt_resp(truncate(cycle_reg));
   endmethod
   method Action dtp_logger_write_cnt(Bit#(8) port_no, Bit#(64) host_timestamp);
      // Check valid port No.
      if (port_no < 4) begin
         lwrite_data_cycle1.enq(BufData{port_no: port_no, data: host_timestamp});
         lwrite_data_cycle2.enq(BufData{port_no: port_no, data: zeroExtend(clocal_reg[port_no])});
      end
   endmethod
   method Action dtp_logger_read_cnt(Bit#(8) port_no);
      if (port_no < 4) begin
         if (lread_data_cycle1[port_no].notEmpty && lread_data_cycle2[port_no].notEmpty) begin
            Bit#(64) remote_message1 = lread_data_cycle1[port_no].first.data;
            Bit#(64) remote_message2 = lread_data_cycle2[port_no].first.data;
            Bit#(53) timestamp = lread_data_timestamp[port_no].first;
            indication.dtp_logger_read_cnt_resp(port_no,
                                                zeroExtend(timestamp),
                                                zeroExtend(remote_message1),
                                                zeroExtend(remote_message2));
            lread_data_cycle1[port_no].deq;
            lread_data_cycle2[port_no].deq;
            lread_data_timestamp[port_no].deq;
         end
      end
   endmethod
   method Action dtp_read_local_cnt(Bit#(8) port_no);
      if (port_no < 4) begin
         indication.dtp_read_local_cnt_resp(port_no, zeroExtend(clocal_reg[port_no]));
      end
   endmethod
   method Action dtp_read_global_cnt();
      indication.dtp_read_global_cnt_resp(zeroExtend(cglobal_reg));
   endmethod
   method Action dtp_set_beacon_interval(Bit#(8) port_no, Bit#(32) interval);
      if (port_no < 4) begin
         beacon_interval[port_no] <= interval;
      end
   endmethod
   method Action dtp_read_beacon_interval(Bit#(8) port_no);
      if (port_no < 4) begin
         indication.dtp_read_beacon_interval_resp(port_no, beacon_interval[port_no]);
      end
   endmethod
   endinterface
endmodule

// Copyright (c) 2015 Cornell University.

// Permission is hereby granted, free of charge, to any person
// obtaining a copy of this software and associated documentation
// files (the "Software"), to deal in the Software without
// restriction, including without limitation the rights to use, copy,
// modify, merge, publish, distribute, sublicense, and/or sell copies
// of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be
// included in all copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
// EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
// MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
// NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
// BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
// ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
// CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

import Clocks::*;
import FIFO::*;
import FIFOF::*;
import Vector::*;
import Pipe::*;
import GetPut::*;
import Probe::*;

typedef struct {
   Bit#(8) port_no;
   Bit#(64) data;
} BufData deriving (Bits, Eq);

interface DtpIfc;
   interface PipeIn#(Bit#(128)) timestamp; // streaming time counter from NetTop.
   interface Vector#(4, PipeOut#(Bit#(53))) fromHost;
   interface Vector#(4, PipeIn#(Bit#(53)))  toHost;
   interface Vector#(4, PipeIn#(Bit#(32)))  delay;
   interface Vector#(4, PipeIn#(Bit#(32)))  state;
   interface Vector#(4, PipeIn#(Bit#(64)))  jumpCount;
   interface Vector#(4, PipeIn#(Bit#(53)))  cLocal;
   interface PipeIn#(Bit#(53)) globalOut;
   interface Vector#(4, PipeOut#(Bit#(32))) interval;
   interface Reset rst;
   interface PipeIn#(Bit#(1)) isSwitch;
endinterface

interface SonicUserRequest;
   method Action dtp_read_version();
   method Action dtp_reset(Bit#(32) len);
   method Action dtp_set_cnt(Bit#(8) port_no, Bit#(64) c);
   method Action dtp_read_delay(Bit#(8) port_no);
   method Action dtp_read_state(Bit#(8) port_no);
   method Action dtp_read_error(Bit#(8) port_no);
   method Action dtp_read_cnt(Bit#(8) cmd);
   method Action dtp_logger_write_cnt(Bit#(8) port_no, Bit#(64) local_cnt);
   method Action dtp_logger_read_cnt(Bit#(8) port_no);
   method Action dtp_read_local_cnt(Bit#(8) port_no);
   method Action dtp_read_global_cnt();
   method Action dtp_set_beacon_interval(Bit#(8) port_no, Bit#(32) interval);
   method Action dtp_read_beacon_interval(Bit#(8) port_no);
/* NOT IMPLEMENTED YET.
   method Action dtp_ctrl_disable();
   method Action dtp_ctrl_enable();
*/
endinterface

interface SonicUserIndication;
   method Action dtp_read_version_resp(Bit#(32) version);
   method Action dtp_read_delay_resp(Bit#(8) port_no, Bit#(32) delay);
   method Action dtp_read_state_resp(Bit#(8) port_no, Bit#(32) state);
   method Action dtp_read_error_resp(Bit#(8) port_no, Bit#(64) jumpc);
   method Action dtp_read_cnt_resp(Bit#(64) val);
   method Action dtp_logger_read_cnt_resp(Bit#(8) port_no, Bit#(64) localc, Bit#(64) msg1, Bit#(64) msg2);
   method Action dtp_read_local_cnt_resp(Bit#(8) port_no, Bit#(64) val);
   method Action dtp_read_global_cnt_resp(Bit#(64) val);
   method Action dtp_read_beacon_interval_resp(Bit#(8) port_no, Bit#(32) interval);
endinterface

interface SonicUser;
   interface SonicUserRequest request;
   interface DtpIfc           dtp;
endinterface

module mkSonicUser#(SonicUserIndication indication)(SonicUser);
   let verbose = False;
   Clock defaultClock <- exposeCurrentClock();

   Reg#(Bit#(64))  cycle_count <- mkReg(0);
   Reg#(Bit#(64))  last_count  <- mkReg(0);

   FIFOF#(Bit#(128)) cntFifo <- mkFIFOF();
   Reg#(Bit#(128))   cycle_reg <- mkReg(0);
   Reg#(Bit#(1))     isSwitch_reg <- mkReg(0);
   Vector#(4, Reg#(Bit#(32))) beacon_interval <- replicateM(mkReg(1000)); //default beacon interval is 1000.

   Vector#(4, FIFOF#(Bit#(53))) fromHostFifo <- replicateM(mkSizedFIFOF(4));
   Vector#(4, FIFOF#(Bit#(53))) toHostFifo   <- replicateM(mkSizedFIFOF(4));
   Vector#(4, FIFOF#(Bit#(32))) delayFifo    <- replicateM(mkSizedFIFOF(4));
   Vector#(4, FIFOF#(Bit#(32))) stateFifo    <- replicateM(mkSizedFIFOF(4));
   Vector#(4, FIFOF#(Bit#(64))) jumpCountFifo <- replicateM(mkSizedFIFOF(4));
   Vector#(4, FIFOF#(Bit#(53))) cLocalFifo   <- replicateM(mkSizedFIFOF(4));
   FIFOF#(Bit#(53)) cGlobalFifo <- mkSizedFIFOF(4);
   Vector#(4, FIFOF#(Bit#(32))) intervalFifo <- replicateM(mkSizedFIFOF(4));
   FIFOF#(Bit#(1)) isSwitchFifo <- mkFIFOF();

   Reg#(Bit#(8))  lwrite_port <- mkReg(0);
   FIFOF#(BufData) lwrite_data_cycle1 <- mkSizedFIFOF(8);
   FIFOF#(BufData) lwrite_data_cycle2 <- mkSizedFIFOF(8);
   FIFOF#(void) log_write_cf <- mkFIFOF;

   Vector#(4, FIFOF#(BufData)) lread_data_cycle1 <- replicateM(mkSizedFIFOF(8));
   Vector#(4, FIFOF#(BufData)) lread_data_cycle2 <- replicateM(mkSizedFIFOF(8));

   Reg#(Bit#(28)) dtp_rst_cntr <- mkReg(0);
   MakeResetIfc dtpResetOut <- mkResetSync(0, False, defaultClock);

   rule count;
      cycle_count <= cycle_count + 1;
   endrule

   // clear all fifo on reset
   rule clearOnReset(dtpResetOut.isAsserted);
      for (Integer i=0; i<4; i=i+1) begin
         fromHostFifo[i].clear();
         toHostFifo[i].clear();
         delayFifo[i].clear();
         stateFifo[i].clear();
         jumpCountFifo[i].clear();
         cLocalFifo[i].clear();
         intervalFifo[i].clear();
         lread_data_cycle1[i].clear();
         lread_data_cycle2[i].clear();
      end
      cGlobalFifo.clear();
      cntFifo.clear();
      lwrite_data_cycle1.clear();
      lwrite_data_cycle2.clear();
      log_write_cf.clear();
   endrule

   // dtp_read_cnt
   rule snapshot_dtp_timestamp;
      let v <- toGet(cntFifo).get;
      cycle_reg <= v;
   endrule

   // dtp_read_delay
   Vector#(4, Reg#(Bit#(32))) delay_reg <- replicateM(mkReg(0));
   for (Integer i=0; i<4; i=i+1) begin
      rule snapshot_delay;
         let v <- toGet(delayFifo[i]).get;
         delay_reg[i] <= v;
      endrule
   end

   // dtp_read_state
   Vector#(4, Reg#(Bit#(32))) state_reg <- replicateM(mkReg(0));
   for (Integer i=0; i<4; i=i+1) begin
      rule snapshot_state;
         let v <- toGet(stateFifo[i]).get;
         state_reg[i] <= v;
      endrule
   end

   // dtp_read_error
   Vector#(4, Reg#(Bit#(64))) jumpc_reg <- replicateM(mkReg(0));
   for (Integer i=0; i<4; i=i+1) begin
      rule snapshot_jumpc;
         let v <- toGet(jumpCountFifo[i]).get;
         jumpc_reg[i] <= v;
      endrule
   end

   // dtp_read_local_cnt
   Vector#(4, Reg#(Bit#(53))) clocal_reg <- replicateM(mkReg(0));
   for (Integer i=0; i<4; i=i+1) begin
      rule snapshot_clocal;
         let v <- toGet(cLocalFifo[i]).get;
         clocal_reg[i] <= v;
      endrule
   end

   // dtp_read_global_cnt
   Reg#(Bit#(53)) cglobal_reg <- mkReg(0);
   rule snapshot_cglobal;
      let v <- toGet(cGlobalFifo).get;
      cglobal_reg <= v;
   endrule

   // dtp_set_interval
   for (Integer i=0; i<4; i=i+1) begin
      rule set_interval;
         let v = beacon_interval[i];
         intervalFifo[i].enq(v);
      endrule
   end

   // dtp_logger_write_cnt
   rule log_from_host_cycle1;
      let v = lwrite_data_cycle1.first;
      if (fromHostFifo[v.port_no].notFull) begin
         fromHostFifo[v.port_no].enq({1'b0, truncate(v.data)});
         log_write_cf.enq(?);
         lwrite_data_cycle1.deq;
      end
   endrule
   rule log_from_host_cycle2 (log_write_cf.notEmpty);
      let v = lwrite_data_cycle2.first;
      if (fromHostFifo[v.port_no].notFull) begin
         fromHostFifo[v.port_no].enq({1'b1, truncate(v.data)});
         lwrite_data_cycle2.deq;
         log_write_cf.deq;
      end
   endrule

   for (Integer i=0; i<4; i=i+1) begin
      rule save_host_data (toHostFifo[i].notEmpty);
         Bit#(53) v = toHostFifo[i].first;
         toHostFifo[i].deq;
         case (v[52]) matches
            0: lread_data_cycle1[i].enq(BufData{port_no:fromInteger(i), data:zeroExtend(v[51:0])});
            1: lread_data_cycle2[i].enq(BufData{port_no:fromInteger(i), data:zeroExtend(v[51:0])});
         endcase
      endrule
   end

   rule assert_reset (dtp_rst_cntr != 0);
      dtp_rst_cntr <= dtp_rst_cntr - 1;
      dtpResetOut.assertReset;
   endrule

   rule switch_mode;
      let v <- toGet(isSwitchFifo).get;
      isSwitch_reg <= v;
   endrule

   // Interface to external modules.
   interface dtp = (interface DtpIfc;
      interface timestamp = toPipeIn(cntFifo);
      interface delay     = map(toPipeIn, delayFifo);
      interface state     = map(toPipeIn, stateFifo);
      interface jumpCount = map(toPipeIn, jumpCountFifo);
      interface toHost    = map(toPipeIn, toHostFifo);
      interface fromHost  = map(toPipeOut, fromHostFifo);
      interface cLocal    = map(toPipeIn, cLocalFifo);
      interface globalOut = toPipeIn(cGlobalFifo);
      interface interval  = map(toPipeOut, intervalFifo);
      interface rst       = dtpResetOut.new_rst;
      interface isSwitch  = toPipeIn(isSwitchFifo);
   endinterface);

   // API implementation
   interface SonicUserRequest request;
   method Action dtp_read_version();
      let v = `DtpVersion; //Defined in Makefile as time of compilation.
      indication.dtp_read_version_resp(v);
   endmethod
   method Action dtp_reset(Bit#(32) len);
      Bit#(28) limit = truncate(len) & 28'hFFFFFFF;
      if (limit == 0) limit = 32;
      dtp_rst_cntr <= limit;
   endmethod
   method Action dtp_set_cnt(Bit#(8) port_no, Bit#(64) c);
      //
   endmethod
   method Action dtp_read_delay(Bit#(8) port_no);
      if (port_no < 4) begin
         indication.dtp_read_delay_resp(port_no, truncate(delay_reg[port_no]));
      end
   endmethod
   method Action dtp_read_state(Bit#(8) port_no);
      if (port_no < 4) begin
         indication.dtp_read_state_resp(port_no, state_reg[port_no]);
      end
   endmethod
   method Action dtp_read_error(Bit#(8) port_no);
      if (port_no < 4) begin
         indication.dtp_read_error_resp(port_no, jumpc_reg[port_no]);
      end
   endmethod
   method Action dtp_read_cnt(Bit#(8) port_no);
      indication.dtp_read_cnt_resp(truncate(cycle_reg));
   endmethod
   method Action dtp_logger_write_cnt(Bit#(8) port_no, Bit#(64) host_timestamp);
      // Check valid port No.
      if (port_no < 4) begin
         lwrite_data_cycle1.enq(BufData{port_no: port_no, data: host_timestamp});
         lwrite_data_cycle2.enq(BufData{port_no: port_no, data: zeroExtend(clocal_reg[port_no])});
      end
   endmethod
   method Action dtp_logger_read_cnt(Bit#(8) port_no);
      if (port_no < 4) begin
         if (lread_data_cycle1[port_no].notEmpty && lread_data_cycle2[port_no].notEmpty) begin
            Bit#(64) remote_message1 = lread_data_cycle1[port_no].first.data;
            Bit#(64) remote_message2 = lread_data_cycle2[port_no].first.data;
            Bit#(53) timestamp = 0;
            if (isSwitch_reg == 0) begin // 0 for NIC, 1 for Switch
               timestamp = clocal_reg[port_no];
            end
            else begin
               timestamp = cglobal_reg;
            end

            indication.dtp_logger_read_cnt_resp(port_no,
                                                zeroExtend(timestamp),
                                                zeroExtend(remote_message1),
                                                zeroExtend(remote_message2));
            lread_data_cycle1[port_no].deq;
            lread_data_cycle2[port_no].deq;
         end
      end
   endmethod
   method Action dtp_read_local_cnt(Bit#(8) port_no);
      if (port_no < 4) begin
         indication.dtp_read_local_cnt_resp(port_no, zeroExtend(clocal_reg[port_no]));
      end
   endmethod
   method Action dtp_read_global_cnt();
      indication.dtp_read_global_cnt_resp(zeroExtend(cglobal_reg));
   endmethod
   method Action dtp_set_beacon_interval(Bit#(8) port_no, Bit#(32) interval);
      if (port_no < 4) begin
         beacon_interval[port_no] <= interval;
      end
   endmethod
   method Action dtp_read_beacon_interval(Bit#(8) port_no);
      if (port_no < 4) begin
         indication.dtp_read_beacon_interval_resp(port_no, beacon_interval[port_no]);
      end
   endmethod
   endinterface
endmodule


bugfix: reset rg_tmp after each packet

bugfix: reset rg_tmp after each packet

// Copyright (c) 2016 P4FPGA Project

// Permission is hereby granted, free of charge, to any person
// obtaining a copy of this software and associated documentation
// files (the "Software"), to deal in the Software without
// restriction, including without limitation the rights to use, copy,
// modify, merge, publish, distribute, sublicense, and/or sell copies
// of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be
// included in all copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
// EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
// MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
// NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
// BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
// ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
// CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

// Deparser Template

import BUtils::*;
import BuildVector::*;
import CBus::*;
import ClientServer::*;
import ConfigReg::*;
import Connectable::*;
import DbgDefs::*;
import DefaultValue::*;
import Ethernet::*;
import FIFO::*;
import FIFOF::*;
import FShow::*;
import GetPut::*;
import List::*;
import MIMO::*;
import MatchTable::*;
import PacketBuffer::*;
import Pipe::*;
import PrintTrace::*;
import Register::*;
import SpecialFIFOs::*;
import StmtFSM::*;
import TxRx::*;
import Utils::*;
import Vector::*;

import StructGenerated::*;

// app-specific structs
`define DEPARSER_STRUCT
`include "DeparserGenerated.bsv"
`undef DEPARSER_STRUCT

typedef TDiv#(PktDataWidth, 8) MaskWidth;
typedef TLog#(PktDataWidth) DataSize;
typedef TLog#(TDiv#(PktDataWidth, 8)) MaskSize;
typedef TAdd#(DataSize, 1) NumBits;
typedef TAdd#(MaskSize, 1) NumBytes;

interface Deparser;
   interface PipeIn#(MetadataT) metadata;
   interface PktWriteServer writeServer;
   interface PktWriteClient writeClient;
   method Action set_verbosity (int verbosity);
   method DeparserPerfRec read_perf_info ();
endinterface
module mkDeparser (Deparser);
   Reg#(int) cf_verbosity <- mkConfigRegU;
   function Action dbprint(Integer level, Fmt msg);
      action
      if (cf_verbosity > fromInteger(level)) begin
         $display("(%0d) ", $time, msg);
      end
      endaction
   endfunction

   FIFOF#(EtherData) data_in_ff <- printTimedTraceM("deparse_in_ff", mkFIFOF);
   FIFOF#(EtherData) data_out_ff <- printTimedTraceM("deparse_out_ff", mkFIFOF);
   FIFOF#(MetadataT) meta_in_ff <- printTimedTraceM("deparse_meta_in_ff", mkFIFOF);
   FIFOF#(Maybe#(Bit#(128))) data_ff <- mkDFIFOF(tagged Invalid);
   FIFO#(DeparserState) deparse_state_ff <- printTimedTraceM("deparseState", mkPipelineFIFO());
   Array#(Reg#(Bit#(32))) rg_next_header_len <- mkCReg(3, 0);
   Array#(Reg#(Bit#(32))) rg_buffered <- mkCReg(3, 0); // number of bytes buffered in rg_tmp
   Array#(Reg#(Bit#(32))) rg_processed <- mkCReg(3, 0); // number of bytes in current header that have been sent.
   Array#(Reg#(Bit#(32))) rg_shift_amt <- mkCReg(3, 0); // number of bytes to shift to append new bytes to rg_tmp
   Array#(Reg#(Bit#(512))) rg_tmp <- mkCReg(2, 0);
   Array#(Reg#(Bool)) deparse_done <- mkCReg(2, True);
   Array#(Reg#(Bool)) header_done <- mkCReg(2, True);
   PulseWire w_deparse_header_done <- mkPulseWire();

   // app-specific states
   `define DEPARSER_STATE
   `include "DeparserGenerated.bsv"
   `undef DEPARSER_STATE

   let mask_this_cycle = data_in_ff.first.mask;
   let sop_this_cycle = data_in_ff.first.sop;
   let eop_this_cycle = data_in_ff.first.eop;
   let data_this_cycle = data_in_ff.first.data;
   let meta = meta_in_ff.first;
   function Action report_deparse_action(String msg, Bit#(32) buffered, Bit#(32) processed, Bit#(32) shift, Bit#(512) data);
    action
      if (cf_verbosity > 0) begin
        $display("(%0d) Deparse %s buffered %d %d %d data %h", $time, msg, buffered, processed, shift, data);
      end
    endaction
  endfunction
  function Action fetch_next_header(Bit#(32) len);
    action
      rg_next_header_len[0] <= len;
    endaction
  endfunction
  function Action move_buffered_amt(Bit#(32) len);
    action
      rg_buffered[0] <= rg_buffered[0] + len;
      rg_shift_amt[0] <= rg_shift_amt[0] + len;
    endaction
  endfunction
  function Action succeed_and_next(Bit#(32) len);
    action
      rg_processed[0] <= rg_processed[0] + len;
      rg_buffered[0] <= rg_buffered[0] - len;
    endaction
  endfunction
  function Bit#(max) create_mask(LUInt#(max) count);
    Bit#(max) v = (1 << count) - 1;
    return v;
  endfunction

  rule rl_start_state if (deparse_done[1] && sop_this_cycle);
    rg_tmp[1] <= 0;
    rg_buffered[2] <= 0;
    rg_shift_amt[2] <= 0;
    rg_processed[2] <= 0;
    deparse_done[1] <= False;
    header_done[1] <= False;
    deparse_state_ff.enq(StateDeparseEthernet);
    fetch_next_header(112);
    dbprint(3, $format("start deparse %d", valueOf(SizeOf#(DeparserState))));
  endrule

  rule rl_deparse_payload if (deparse_done[1] && !sop_this_cycle);
    let v = data_in_ff.first;
    dbprint(3, $format("Dep payload", fshow(v)));
    data_in_ff.deq;
    dbprint(3, $format("Dep payload shift amt %d", rg_shift_amt[1]));
    if (rg_shift_amt[1] != 0) begin
      Bit#(128) data_mask = create_mask(cExtend(rg_shift_amt[1]));
      Bit#(16) mask_out = create_mask(cExtend(rg_shift_amt[1] >> 3));
      let data = EtherData { sop: v.sop, eop: v.eop, data: truncate(rg_tmp[1]) & data_mask, mask: mask_out};
      data_out_ff.enq(data);
      dbprint(3, $format("Deparser payload2 rg_shift_amt=%d", rg_shift_amt[1], fshow(data)));
      rg_shift_amt[1] <= 0;
    end
    else begin
      dbprint(3, $format("Deparser payload1 ", fshow(v)));
      data_out_ff.enq(v);
    end
  endrule

  rule rl_data_ff_load if (!deparse_done[1] && (rg_buffered[2] < rg_next_header_len[2]));
    dbprint(3, $format("dequeue data_in_ff %d < %d", rg_buffered[2], rg_next_header_len[2]));
    data_in_ff.deq;
  endrule

  rule rl_deparse_send if (!deparse_done[1] && (rg_processed[1] > 0));
    let amt = 128;
    if (rg_processed[1] < 128) begin
       amt = rg_processed[1];
    end
    let v = data_in_ff.first;
    dbprint(3, $format("Deparser ", fshow(v)));
    Bit#(128) data_out = truncate(rg_tmp[1] & create_mask(cExtend(amt)));
    Bit#(16) mask_out = create_mask(cExtend(amt >> 3));
    let data = EtherData { sop: sop_this_cycle, eop: False, data: data_out, mask: mask_out };
    rg_tmp[1] <= rg_tmp[1] >> amt;
    rg_processed[1] <= rg_processed[1] - amt;
    rg_shift_amt[1] <= rg_shift_amt[1] - amt;
    data_out_ff.enq(data);
    dbprint(3, $format("Deparser rg_processed=%d rg_shift_amt=%d amt=%d", rg_processed[1], rg_shift_amt[1], amt, fshow(data)));
  endrule

  // wait till all processed bits are sent, cont. to send payload.
  // some data are buffered not processed.
  rule rl_wait_till_processed_done if (header_done[1] && (rg_processed[1] == 0));
    deparse_done[1] <= True;
  endrule

  `define DEPARSER_RULES
  `include "DeparserGenerated.bsv"
  `undef DEPARSER_RULES

  interface metadata = toPipeIn(meta_in_ff);
  interface PktWriteServer writeServer;
    interface writeData = toPut(data_in_ff);
  endinterface
  interface PktWriteClient writeClient;
    interface writeData = toGet(data_out_ff);
  endinterface
  method Action set_verbosity (int verbosity);
    cf_verbosity <= verbosity;
  endmethod
endmodule

// Copyright (c) 2016 P4FPGA Project

// Permission is hereby granted, free of charge, to any person
// obtaining a copy of this software and associated documentation
// files (the "Software"), to deal in the Software without
// restriction, including without limitation the rights to use, copy,
// modify, merge, publish, distribute, sublicense, and/or sell copies
// of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be
// included in all copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
// EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
// MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
// NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
// BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
// ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
// CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

// Deparser Template

import BUtils::*;
import BuildVector::*;
import CBus::*;
import ClientServer::*;
import ConfigReg::*;
import Connectable::*;
import DbgDefs::*;
import DefaultValue::*;
import Ethernet::*;
import FIFO::*;
import FIFOF::*;
import FShow::*;
import GetPut::*;
import List::*;
import MIMO::*;
import MatchTable::*;
import PacketBuffer::*;
import Pipe::*;
import PrintTrace::*;
import Register::*;
import SpecialFIFOs::*;
import StmtFSM::*;
import TxRx::*;
import Utils::*;
import Vector::*;

import StructGenerated::*;

// app-specific structs
`define DEPARSER_STRUCT
`include "DeparserGenerated.bsv"
`undef DEPARSER_STRUCT

typedef TDiv#(PktDataWidth, 8) MaskWidth;
typedef TLog#(PktDataWidth) DataSize;
typedef TLog#(TDiv#(PktDataWidth, 8)) MaskSize;
typedef TAdd#(DataSize, 1) NumBits;
typedef TAdd#(MaskSize, 1) NumBytes;

interface Deparser;
   interface PipeIn#(MetadataT) metadata;
   interface PktWriteServer writeServer;
   interface PktWriteClient writeClient;
   method Action set_verbosity (int verbosity);
   method DeparserPerfRec read_perf_info ();
endinterface
module mkDeparser (Deparser);
   Reg#(int) cf_verbosity <- mkConfigRegU;
   function Action dbprint(Integer level, Fmt msg);
      action
      if (cf_verbosity > fromInteger(level)) begin
         $display("(%0d) ", $time, msg);
      end
      endaction
   endfunction

   FIFOF#(EtherData) data_in_ff <- printTimedTraceM("in_ff", mkFIFOF);
   FIFOF#(EtherData) data_out_ff <- printTimedTraceM("out_ff", mkFIFOF);
   FIFOF#(MetadataT) meta_in_ff <- printTimedTraceM("deparse_meta_in_ff", mkFIFOF);
   FIFOF#(Maybe#(Bit#(128))) data_ff <- mkDFIFOF(tagged Invalid);
   FIFO#(DeparserState) deparse_state_ff <- printTimedTraceM("deparseState", mkPipelineFIFO());
   Array#(Reg#(Bit#(32))) rg_next_header_len <- mkCReg(3, 0);
   Array#(Reg#(Bit#(32))) rg_buffered <- mkCReg(3, 0); // number of bytes buffered in rg_tmp
   Array#(Reg#(Bit#(32))) rg_processed <- mkCReg(3, 0); // number of bytes in current header that have been sent.
   Array#(Reg#(Bit#(32))) rg_shift_amt <- mkCReg(3, 0); // number of bytes to shift to append new bytes to rg_tmp
   Array#(Reg#(Bit#(512))) rg_tmp <- mkCReg(2, 0);
   Array#(Reg#(Bool)) deparse_done <- mkCReg(2, True);
   Array#(Reg#(Bool)) header_done <- mkCReg(2, True);
   PulseWire w_deparse_header_done <- mkPulseWire();

   // app-specific states
   `define DEPARSER_STATE
   `include "DeparserGenerated.bsv"
   `undef DEPARSER_STATE

   let mask_this_cycle = data_in_ff.first.mask;
   let sop_this_cycle = data_in_ff.first.sop;
   let eop_this_cycle = data_in_ff.first.eop;
   let data_this_cycle = data_in_ff.first.data;
   let meta = meta_in_ff.first;
   function Action report_deparse_action(String msg, Bit#(32) buffered, Bit#(32) processed, Bit#(32) shift, Bit#(512) data);
    action
      if (cf_verbosity > 0) begin
        $display("(%0d) Deparse %s buffered %d %d %d data %h", $time, msg, buffered, processed, shift, data);
      end
    endaction
  endfunction
  function Action fetch_next_header(Bit#(32) len);
    action
      rg_next_header_len[0] <= len;
    endaction
  endfunction
  function Action move_buffered_amt(Bit#(32) len);
    action
      rg_buffered[0] <= rg_buffered[0] + len;
      rg_shift_amt[0] <= rg_shift_amt[0] + len;
    endaction
  endfunction
  function Action succeed_and_next(Bit#(32) len);
    action
      rg_processed[0] <= rg_processed[0] + len;
      rg_buffered[0] <= rg_buffered[0] - len;
    endaction
  endfunction
  function Bit#(max) create_mask(LUInt#(max) count);
    Bit#(max) v = (1 << count) - 1;
    return v;
  endfunction

  rule rl_start_state if (deparse_done[1] && sop_this_cycle);
    rg_buffered[2] <= 0;
    rg_shift_amt[2] <= 0;
    rg_processed[2] <= 0;
    deparse_done[1] <= False;
    header_done[1] <= False;
    deparse_state_ff.enq(StateDeparseEthernet);
    fetch_next_header(112);
    dbprint(3, $format("start deparse %d", valueOf(SizeOf#(DeparserState))));
  endrule

  rule rl_deparse_payload if (deparse_done[1] && !sop_this_cycle);
    let v = data_in_ff.first;
    dbprint(3, $format("Dep payload", fshow(v)));
    data_in_ff.deq;
    dbprint(3, $format("Dep payload shift amt %d", rg_shift_amt[1]));
    if (rg_shift_amt[1] != 0) begin
      Bit#(128) data_mask = create_mask(cExtend(rg_shift_amt[1]));
      Bit#(16) mask_out = create_mask(cExtend(rg_shift_amt[1] >> 3));
      let data = EtherData { sop: v.sop, eop: v.eop, data: truncate(rg_tmp[1]) & data_mask, mask: mask_out};
      data_out_ff.enq(data);
      dbprint(3, $format("Deparser payload2 rg_shift_amt=%d", rg_shift_amt[1], fshow(data)));
      rg_shift_amt[1] <= 0;
    end
    else begin
      dbprint(3, $format("Deparser payload1 ", fshow(v)));
      data_out_ff.enq(v);
    end
  endrule

  rule rl_data_ff_load if (!deparse_done[1] && (rg_buffered[2] < rg_next_header_len[2]));
    dbprint(3, $format("dequeue data_in_ff %d < %d", rg_buffered[2], rg_next_header_len[2]));
    data_in_ff.deq;
  endrule

  rule rl_deparse_send if (!deparse_done[1] && (rg_processed[1] > 0));
    let amt = 128;
    if (rg_processed[1] < 128) begin
       amt = rg_processed[1];
    end
    let v = data_in_ff.first;
    dbprint(3, $format("Deparser ", fshow(v)));
    Bit#(128) data_out = truncate(rg_tmp[1] & create_mask(cExtend(amt)));
    Bit#(16) mask_out = create_mask(cExtend(amt >> 3));
    let data = EtherData { sop: sop_this_cycle, eop: False, data: data_out, mask: mask_out };
    rg_tmp[1] <= rg_tmp[1] >> amt;
    rg_processed[1] <= rg_processed[1] - amt;
    rg_shift_amt[1] <= rg_shift_amt[1] - amt;
    data_out_ff.enq(data);
    dbprint(3, $format("Deparser rg_processed=%d rg_shift_amt=%d amt=%d", rg_processed[1], rg_shift_amt[1], amt, fshow(data)));
  endrule

  // wait till all processed bits are sent, cont. to send payload.
  // some data are buffered not processed.
  rule rl_wait_till_processed_done if (header_done[1] && (rg_processed[1] == 0));
    deparse_done[1] <= True;
  endrule

  `define DEPARSER_RULES
  `include "DeparserGenerated.bsv"
  `undef DEPARSER_RULES

  interface metadata = toPipeIn(meta_in_ff);
  interface PktWriteServer writeServer;
    interface writeData = toPut(data_in_ff);
  endinterface
  interface PktWriteClient writeClient;
    interface writeData = toGet(data_out_ff);
  endinterface
  method Action set_verbosity (int verbosity);
    cf_verbosity <= verbosity;
  endmethod
endmodule