//
//    FemtoMSP430 - A MSP430 processor for FPGAs
//    Copyright (C) 2025  Matthias Koch
//
//    This program is free software: you can redistribute it and/or modify
//    it under the terms of the GNU General Public License as published by
//    the Free Software Foundation, either version 3 of the License, or
//    (at your option) any later version.
//
//    This program is distributed in the hope that it will be useful,
//    but WITHOUT ANY WARRANTY; without even the implied warranty of
//    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//    GNU General Public License for more details.
//
//    You should have received a copy of the GNU General Public License
//    along with this program.  If not, see <http://www.gnu.org/licenses/>.
//

module femtomsp430
(
  input wire clk,
  input wire resetq,

  output reg  [15:0] mem_addr,

  output reg  [15:0] mem_wdata,
  output reg   [1:0] mem_wmask,

  input  wire [15:0] mem_rdata,
  output reg         mem_rstrb,

  input  wire interrupt_request,

  input  wire mem_busy
);

parameter     reset_vector = 16'hFFFE;
parameter interrupt_vector = 16'hFFF4;

// A very simple implementation of the MSP430 instruction set,
// optimised for tiny size in FPGA, not for cycle-accurate performance.

// Unlike other MSP430 softcores, this one supports memory busy signaling,
// and comes with a bus interface very similar to the FemtoRV32 cores.

// -----------------------------------------------------------------------------
//  Register set and flags
// -----------------------------------------------------------------------------

reg [15:0] registers [15:0];
reg [15:0] source;
reg [15:0] destination;

localparam PC = 0;  wire [15:0] pc = registers[PC];
localparam SP = 1;  wire [15:0] sp = registers[SP];
localparam SR = 2;  wire [15:0] sr = registers[SR];
localparam CG = 3;

localparam FLAG_C = 0; wire flag_c = sr[FLAG_C];
localparam FLAG_Z = 1; wire flag_z = sr[FLAG_Z];
localparam FLAG_N = 2; wire flag_n = sr[FLAG_N];
localparam FLAG_V = 8; wire flag_v = sr[FLAG_V];

localparam GIE    = 3; wire gie    = sr[GIE];
localparam CPUOFF = 4; wire cpuoff = sr[CPUOFF];

wire [15:0] pcplus2  = pc + 2;
wire [15:0] spplus2  = sp + 2;
wire [15:0] spminus2 = sp - 2;

// -----------------------------------------------------------------------------
//  Instruction field decoding
// -----------------------------------------------------------------------------

reg  [15:0] insn;
wire [15:0] instruction = insn;

wire       bw = instruction[6];

wire [1:0] as = instruction[5:4];
wire       ad = instruction[7];

wire [3:0] rs = is_single_op ? rd : instruction[11:8];
wire [3:0] rd = instruction[3:0];

wire destination_is_register = is_single_op ? (as == 0) : (ad == 0);

wire is_jmp = instruction[15:13] == 3'b001;

wire OP_JNE = is_jmp & (instruction[12:10] == 3'b000);
wire OP_JEQ = is_jmp & (instruction[12:10] == 3'b001);
wire OP_JNC = is_jmp & (instruction[12:10] == 3'b010);
wire OP_JC  = is_jmp & (instruction[12:10] == 3'b011);
wire OP_JN  = is_jmp & (instruction[12:10] == 3'b100);
wire OP_JGE = is_jmp & (instruction[12:10] == 3'b101);
wire OP_JL  = is_jmp & (instruction[12:10] == 3'b110);
wire OP_JMP = is_jmp & (instruction[12:10] == 3'b111);

wire is_single_op = instruction[15:13] == 3'b000;

wire OP_RRC  = is_single_op & (instruction[9:7] == 3'b000);
wire OP_SWPB = is_single_op & (instruction[9:7] == 3'b001);
wire OP_RRA  = is_single_op & (instruction[9:7] == 3'b010);
wire OP_SXT  = is_single_op & (instruction[9:7] == 3'b011);
wire OP_PUSH = is_single_op & (instruction[9:7] == 3'b100);
wire OP_CALL = is_single_op & (instruction[9:7] == 3'b101);
wire OP_RETI = is_single_op & (instruction[9:7] == 3'b110);

wire OP_MOV  = instruction[15:12] == 4'b0100;
wire OP_ADD  = instruction[15:12] == 4'b0101;
wire OP_ADDC = instruction[15:12] == 4'b0110;
wire OP_SUBC = instruction[15:12] == 4'b0111;
wire OP_SUB  = instruction[15:12] == 4'b1000;
wire OP_CMP  = instruction[15:12] == 4'b1001;
wire OP_DADD = instruction[15:12] == 4'b1010;
wire OP_BIT  = instruction[15:12] == 4'b1011;
wire OP_BIC  = instruction[15:12] == 4'b1100;
wire OP_BIS  = instruction[15:12] == 4'b1101;
wire OP_XOR  = instruction[15:12] == 4'b1110;
wire OP_AND  = instruction[15:12] == 4'b1111;

// -----------------------------------------------------------------------------
//  Jumps
// -----------------------------------------------------------------------------

wire [10:0] jump_offset  = { instruction[9:0], 1'b0 };
wire [15:0] jump_address = $signed(pc) + $signed(jump_offset);

wire jmp_not_taken  = ( OP_JL  & ~(flag_v^flag_n) ) |
                      ( OP_JGE &  (flag_v^flag_n) ) |
                      ( OP_JN  &  ~flag_n         ) |
                      ( OP_JC  &  ~flag_c         ) |
                      ( OP_JNC &   flag_c         ) |
                      ( OP_JEQ &  ~flag_z         ) |
                      ( OP_JNE &   flag_z         ) ;

// -----------------------------------------------------------------------------
//  Possible states of the processor
// -----------------------------------------------------------------------------

reg [3:0] state = 0;

localparam STATE_RESET            =  0;
localparam STATE_IRQ_0            =  1;
localparam STATE_IRQ_1            =  2;
localparam STATE_RETI             =  3;
localparam STATE_FETCH_PC         =  4;
localparam STATE_FETCH_OPCODE     =  5;
localparam STATE_STORE_OPCODE     =  6;
localparam STATE_DECODE           =  7;
localparam STATE_MODE_ABSOLUTE    =  8;
localparam STATE_MODE_REG_INDEXED =  9;
localparam STATE_FETCH_DATA       = 10;
localparam STATE_PUSH_CALL        = 11;
localparam STATE_FETCH_DEST_0     = 12;
localparam STATE_FETCH_DEST_1     = 13;
localparam STATE_FETCH_DEST_2     = 14;
localparam STATE_ALU_EXECUTE      = 15;

wire [3:0] execute_state = OP_PUSH | OP_CALL ? STATE_PUSH_CALL    :
                          ~is_single_op & ad ? STATE_FETCH_DEST_0 :
                                               STATE_ALU_EXECUTE  ;

// -----------------------------------------------------------------------------
//  Decimal BCD addition
// -----------------------------------------------------------------------------

// The DADD implementation is taken from OpenMSP430 by Olivier Girard:
// https://github.com/olgirard/openmsp430/blob/master/core/rtl/verilog/omsp_alu.v

function [4:0] bcd_add;

   input [3:0] X;
   input [3:0] Y;
   input       C_;

   reg   [4:0] Z_;
   begin
      Z_ = {1'b0,X}+{1'b0,Y}+{4'b0000,C_};
      if (Z_<5'd10) bcd_add = Z_;
      else          bcd_add = Z_+5'd6;
   end

endfunction

// Decimal adder (DADD)
wire  [4:0] alu_dadd0 = bcd_add(source[3:0],   destination[3:0],   flag_c);
wire  [4:0] alu_dadd1 = bcd_add(source[7:4],   destination[7:4],   alu_dadd0[4]);
wire  [4:0] alu_dadd2 = bcd_add(source[11:8],  destination[11:8],  alu_dadd1[4]);
wire  [4:0] alu_dadd3 = bcd_add(source[15:12], destination[15:12], alu_dadd2[4]);
wire [16:0] alu_dadd  = {alu_dadd3, alu_dadd2[3:0], alu_dadd1[3:0], alu_dadd0[3:0]};

// -----------------------------------------------------------------------------
//  All other ALU functions
// -----------------------------------------------------------------------------

wire [15:0] alu_addc_1 = OP_ADD | OP_ADDC ? source : source ^ (bw ? 8'hFF : 16'hFFFF);
wire [15:0] alu_addc_2 = destination;
wire        alu_addc_c = OP_ADD ? 0 : OP_SUB | OP_CMP ? 1 : flag_c;

wire [16:0] alu_addc = alu_addc_1 + alu_addc_2 + alu_addc_c;

wire [16:0] alu_out =

  ( OP_RRC  ?  bw ? {8'h00,    flag_c, source[7:1]} : {    flag_c, source[15:1]}  : 17'h0000) |
  ( OP_RRA  ?  bw ? {8'h00, source[7], source[7:1]} : {source[15], source[15:1]}  : 17'h0000) |
  ( OP_SWPB ? {source[7:0], source[15:8]}                                         : 17'h0000) |
  ( OP_SXT  ?  $signed(source[7:0])                                               : 17'h0000) |

  ( OP_MOV  ? source                 : 17'h00000 ) |
  ( OP_ADD  |
    OP_ADDC |
    OP_SUBC |
    OP_SUB  |
    OP_CMP  ? alu_addc               : 17'h00000 ) |
  ( OP_DADD ? alu_dadd               : 17'h00000 ) |
  ( OP_BIT  ? destination &  source  : 17'h00000 ) |
  ( OP_BIC  ? destination & ~source  : 17'h00000 ) |
  ( OP_BIS  ? destination |  source  : 17'h00000 ) |
  ( OP_XOR  ? destination ^  source  : 17'h00000 ) |
  ( OP_AND  ? destination &  source  : 17'h00000 ) ;

// -----------------------------------------------------------------------------
//  Generate flags on the ALU output
// -----------------------------------------------------------------------------

wire  V_xor = bw ?    source[ 7] &  destination[ 7] :
                      source[15] &  destination[15] ;

wire  V     = bw ? (alu_addc_1[ 7] == alu_addc_2[ 7]) & (alu_addc_1[ 7] ^ alu_addc[ 7]) :
                   (alu_addc_1[15] == alu_addc_2[15]) & (alu_addc_1[15] ^ alu_addc[15]) ;

wire  N     = bw ?   alu_out[7]   :   alu_out[15];
wire  Z     = bw ? ~|alu_out[7:0] : ~|alu_out[15:0];
wire  C     = bw ?   alu_out[8]   :   alu_out[16];

// -----------------------------------------------------------------------------
//  Aligned memory access
// -----------------------------------------------------------------------------

wire [15:0] mem_rdata_aligned = bw ? mem_addr[0] ? { 8'h00, mem_rdata[15:8] } : { 8'h00, mem_rdata[7:0] } : mem_rdata;
wire [15:0] mem_wdata_aligned = bw & mem_addr[0] ? {alu_out[7:0], alu_out[7:0]} : alu_out[15:0];
wire  [1:0] mem_wmask_aligned = bw ? mem_addr[0] ? 2'b10 :  2'b01 : 2'b11;

// -----------------------------------------------------------------------------
//  Interrupt logic
// -----------------------------------------------------------------------------

  // Remember interrupt requests as they are not checked for every cycle
  reg interrupt_request_sticky = 0;

  // Processor shows that current pending interrupt has been accepted
  reg interrupt_accepted = 0;

  // For executing one more instruction of the main program before entering handler again
  reg interrupt_locked   = 0;

  // Interrupt enable and lock logic
  wire interrupt = interrupt_request_sticky & gie & ~interrupt_locked;

  always @(posedge clk)
  begin
    // If current interrupt is accepted, there already might be the next one, which should not be missed:
    interrupt_request_sticky <= resetq & (interrupt_request | (interrupt_request_sticky & ~interrupt_accepted));
  end

// -----------------------------------------------------------------------------
//  Main processor state machine
// -----------------------------------------------------------------------------

reg waitstate = 0;

always @(posedge clk)
begin

  // Pulse the read and write wires for one clock cycle only for each access
  mem_rstrb  <= 0;
  mem_wmask  <= 2'b00;

  // For every state of the CPU, wait one clock cycle for memory access
  waitstate <= ~waitstate | mem_busy;

  // Default: No interrupt accepted. Can be pulsed in the state machine upon handler entry
  interrupt_accepted <= 0;

  if (!resetq) state <= STATE_RESET;  // Begin with request initial PC from vector table
  else if (~waitstate)               // Do nothing until memory finishes being busy
  case (state)

    // -------------------------------------------------------------------------
    //  Initialisation, interrupt entry and return
    // -------------------------------------------------------------------------

    STATE_RESET:
      begin
        mem_rstrb <= 1;
        mem_addr  <= reset_vector; // Fetch entry from vector table
        registers[SR] <= 0;
        state <= STATE_FETCH_PC;
      end

    STATE_IRQ_0: // Interrupt entry sequence continues with push sr to stack and clear it
      begin
        mem_wmask <= 2'b11;
        mem_wdata <= sr;
        mem_addr      <= spminus2;
        registers[SP] <= spminus2;
        registers[SR] <= 0; // Disable further interrupts and clear cpuoff
        state <= STATE_IRQ_1;
      end

    STATE_IRQ_1: // Fetch interrupt entry point from vector table
      begin
        mem_rstrb <= 1;
        mem_addr  <= interrupt_vector; // Fetch entry from vector table
        state <= STATE_FETCH_PC;
      end

    STATE_RETI:
      begin
        interrupt_locked <= 1; // Execute at least one instruction of the main program before accepting the next interrupt.
        registers[SR] <= mem_rdata;
        mem_rstrb     <= 1;
        mem_addr      <= sp;
        registers[SP] <= spplus2;
        state <= STATE_FETCH_PC;
      end

    STATE_FETCH_PC:
      begin
        registers[PC] <= mem_rdata;
        state <= STATE_FETCH_OPCODE;
      end

    // -------------------------------------------------------------------------
    //  Fetch instruction
    // -------------------------------------------------------------------------

    STATE_FETCH_OPCODE:
      begin
        interrupt_locked <= 0; // After this instruction, the next interrupt entry is possible.
                              // This allows using the interrupt line set continuously high as a creative single-step mode.

        if (interrupt) // Interrupt entry sequence starts with push pc to stack
        begin
          mem_wmask <= 2'b11;
          mem_wdata <= pc;
          mem_addr      <= spminus2;
          registers[SP] <= spminus2;
          interrupt_accepted <= 1;
          state <= STATE_IRQ_0;
        end
        else

        if (cpuoff)
        begin
          // Do nothing and wait for an interrupt if the CPU is halted
        end
        else

        begin
          mem_rstrb <= 1;
          mem_addr      <= pc;
          registers[PC] <= pcplus2;
          state <= STATE_STORE_OPCODE;
        end
      end

    STATE_STORE_OPCODE:
      begin
        insn <= mem_rdata;
        state <= STATE_DECODE;
        destination <= mem_rdata[6] ? registers[mem_rdata[3:0]][7:0] : registers[mem_rdata[3:0]]; // Load destination from the register set, even if we might not need it
      end

    // -------------------------------------------------------------------------
    //  Decode instruction
    // -------------------------------------------------------------------------

    STATE_DECODE:
      begin

        if (is_jmp)
        begin
          if (~jmp_not_taken) registers[PC] <= jump_address;
          state <= STATE_FETCH_OPCODE;
        end
        else

        if (OP_RETI)
        begin
          mem_rstrb <= 1;
          mem_addr      <= sp;
          registers[SP] <= spplus2;
          state <= STATE_RETI;
        end
        else

        begin
          if (rs == SR)
          begin
            state <= execute_state;
            case (as)
              0: source <= bw ? sr[7:0] : sr;
              1: begin mem_rstrb <= 1; mem_addr <= pc; state <= STATE_MODE_ABSOLUTE; end
              2: source <= 4;
              3: source <= 8;
            endcase
          end
          else

          if (rs == CG)
          begin
            state <= execute_state;
            case (as)
              0: source <= 0;
              1: source <= 1;
              2: source <= 2;
              3: source <= bw ? 8'hFF : 16'hFFFF;
            endcase
          end
          else

          begin
            source <= bw ? registers[rs][7:0] : registers[rs];

            case (as)
              0:begin // reg
                  state <= execute_state;
                end

              1:begin // x(reg)
                  mem_rstrb <= 1;
                  mem_addr  <= pc;
                  state <= STATE_MODE_REG_INDEXED;
                end

              2:begin // @reg
                  mem_rstrb <= 1;
                  mem_addr  <= registers[rs];
                  state <= STATE_FETCH_DATA;
                end

              3:begin // @reg+
                  registers[rs] <= registers[rs] + (bw & (rs != PC) ? 1 : 2); // Keep PC even
                  mem_rstrb <= 1;
                  mem_addr  <= registers[rs];
                  state <= STATE_FETCH_DATA;
                end
            endcase
          end

        end

      end

    // -------------------------------------------------------------------------
    //  Fetch source operand from memory
    // -------------------------------------------------------------------------

    STATE_MODE_ABSOLUTE:
      begin
        registers[PC] <= pcplus2;
        mem_addr <= mem_rdata;
        mem_rstrb <= 1;
        state <= STATE_FETCH_DATA;
      end

    STATE_MODE_REG_INDEXED:
      begin
        registers[PC] <= pcplus2;
        mem_addr <= $signed(registers[rs]) + $signed(mem_rdata);
        mem_rstrb <= 1;
        state <= STATE_FETCH_DATA;
      end

    STATE_FETCH_DATA:
      begin
        source <= mem_rdata_aligned;
        state <= execute_state;
      end

    // -------------------------------------------------------------------------
    //  One-operand push and call instructions
    // -------------------------------------------------------------------------

    STATE_PUSH_CALL:
      begin
        if (OP_CALL) registers[PC] <= source;

        mem_wmask <= 2'b11;
        mem_wdata <= OP_CALL ? pc : bw ? {8'h00, source[7:0]} : source[15:0];
        mem_addr      <= spminus2;
        registers[SP] <= spminus2;
        state <= STATE_FETCH_OPCODE;
      end

    // -------------------------------------------------------------------------
    //  Fetch destination for two-operand instructions from memory
    // -------------------------------------------------------------------------

    STATE_FETCH_DEST_0: // Fetch operand for absolute/indexed memory address
      begin
        mem_addr <= pc;
        mem_rstrb <= 1;
        state <= STATE_FETCH_DEST_1;
      end

    STATE_FETCH_DEST_1: // The operand containing the index or absolute address arrives
      begin
        registers[PC] <= pcplus2;
        mem_addr <= (rd == SR) ? mem_rdata : registers[rd] + mem_rdata;

        if (OP_MOV) state <= STATE_ALU_EXECUTE; // MOV does not fetch destination, just prepare address
        else
        begin
          mem_rstrb <= 1;
          state <= STATE_FETCH_DEST_2;
        end
      end

    STATE_FETCH_DEST_2: // Destination operand data incoming
      begin
        destination <= mem_rdata_aligned;
        state <= STATE_ALU_EXECUTE;
      end

    // -------------------------------------------------------------------------
    //  Both single and double operand instructions are executed here
    // -------------------------------------------------------------------------

    STATE_ALU_EXECUTE:
      begin

        if (~(OP_SWPB | OP_MOV | OP_BIC | OP_BIS))
        begin
          registers[SR][FLAG_V] <=                         OP_XOR ? V_xor :
                     OP_ADD | OP_ADDC | OP_SUBC | OP_SUB | OP_CMP ? V     :
                                                                    0     ;
          registers[SR][FLAG_N] <= N;
          registers[SR][FLAG_Z] <= Z;
          registers[SR][FLAG_C] <= OP_RRA | OP_RRC ? source[0] : OP_SXT | OP_BIT | OP_XOR | OP_AND ? ~Z : C;
        end

        if (~(OP_CMP | OP_BIT))
        if (destination_is_register) registers[rd] <= bw ? alu_out[7:0] : alu_out[15:0];
        else
        begin
          mem_wdata <= mem_wdata_aligned;
          mem_wmask <= mem_wmask_aligned;
        end

        state <= STATE_FETCH_OPCODE;
      end

  endcase
end

endmodule
