TMDS encoder in Chisel 2/3

TmdsEncoder.scala

package DviController

import Chisel._

class TmdsEncoder extends Module {
  val io = new Bundle {
    val disp_ena = Bool(INPUT)
    val control = UInt(INPUT, 2)
    val d_in = UInt(INPUT, 8)
    val q_out = UInt(OUTPUT, 10)
  }

  io.q_out := UInt(0)
  val q_m = Wire(UInt(width = 9))
  val ones_din = Wire(UInt(width = 4))
  val ones_q_m = Wire(UInt(width = 4))
  val diff_q_m = Wire(SInt(width = 8))
  val disparity = Wire(SInt(width = 8))
  val disparity_reg = Reg(SInt(0, width = 8))

  ones_din := PopCount(io.d_in)

  val q_ms = Vec.fill(9) { Wire(Bool()) }
  for (n <- 0 to 8)
    q_ms(n) := Bool(false)
  q_ms(0) := io.d_in(0)
  when (ones_din > UInt(4) || (ones_din === UInt(4) && io.d_in(0) === UInt(0))) {
    for (n <- 1 to 7)
      q_ms(n) := !(q_ms(n-1) ^ io.d_in(n))
    q_ms(8) := Bool(false)
  } .otherwise {
    for (n <- 1 to 7)
      q_ms(n) := (q_ms(n-1) ^ io.d_in(n))
    q_ms(8) := Bool(true)
  }
  q_m := q_ms.toBits().toUInt()

  ones_q_m := PopCount(q_m(7, 0))
  diff_q_m := (UInt(2) * ones_q_m) - SInt(8)

  when (io.disp_ena === Bool(true)) {
    when (disparity_reg === SInt(0) || ones_q_m === UInt(4)) {
      when (q_m(8) === Bool(false)) {
        io.q_out := Cat(~q_m(8), q_m(8), ~q_m(7, 0))
        disparity := disparity_reg - diff_q_m
      } .otherwise {
        io.q_out := Cat(~q_m(8), q_m(8, 0))
        disparity := disparity_reg + diff_q_m
      }
    } .otherwise {
      when ((disparity_reg > SInt(0) && ones_q_m > UInt(4))
         || (disparity_reg < SInt(0) && ones_q_m < UInt(4))) {
        io.q_out := Cat(Bool(true), q_m(8), ~q_m(7, 0))
        when (q_m(8) === Bool(false)) {
          disparity := disparity_reg - diff_q_m
        } .otherwise {
          disparity := disparity_reg - diff_q_m + SInt(2)
        }
      } .otherwise {
        io.q_out := Cat(Bool(false), q_m(8, 0))
        when (q_m(8) === Bool(false)) {
          disparity := disparity_reg + diff_q_m - SInt(2)
        } .otherwise {
          disparity := disparity_reg + diff_q_m
        }
      }
    }
  } .otherwise {
    switch (io.control) {
      is (UInt("b00")) { io.q_out := BitPat("b1101010100") }
      is (UInt("b01")) { io.q_out := BitPat("b0010101011") }
      is (UInt("b10")) { io.q_out := BitPat("b0101010100") }
      is (UInt("b11")) { io.q_out := BitPat("b1010101011") }
    }
    disparity := SInt(0)
  }
  disparity_reg := disparity
}

class TmdsEncoderTester(c: TmdsEncoder) extends Tester(c) {
  // test disp_ena == false
  val m = Map(0 -> 0x354, 1 -> 0xab, 2 -> 0x154, 3 -> 0x2ab)
  for ((k, v) <- m) {
    poke(c.io.disp_ena, false)
    poke(c.io.control, k)
    expect(c.io.q_out, v)
    step(1)
  }
  poke(c.io.disp_ena, true)
  var disparity = 0
  for (n <- 0 to 255) {
    poke(c.io.d_in, n)
    val (q_out, disparity_out) = TmdsEncoderInScala.tmdsEncode(n, disparity)
    expect(c.io.q_out, q_out)
    expect(c.disparity, disparity_out)
    step(1)
    expect(c.disparity_reg, disparity_out)
    disparity = disparity_out
  }
}

object SingleModuleTmdsEncoder extends Runner[TmdsEncoder] {
  def main(args:Array[String]): Unit =
    mainMethod(args)(() => new TmdsEncoder())(c => new TmdsEncoderTester(c))
}