cppextendedrisc16.cpp

Go to the documentation of this file.

/* -*- coding: latin-1 -*- */

/** \file cpprisc16/cppextendedrisc16.cpp (March 12, 2017)
 * \brief Extended instructions set.
 *
 * Piece of cpprisc16.
 * https://bitbucket.org/OPiMedia/cpprisc16
 *
 * GPLv3 --- Copyright (C) 2017 Olivier Pirson
 * http://www.opimedia.be/
 */

#include "cppextendedrisc16.hpp"

#include "assert.hpp"


namespace cpprisc16 {
  void
  x_add32(unsigned int a2, unsigned int a1, unsigned int b2, unsigned int b1,
          unsigned int tmp1, unsigned int tmp2, unsigned int tmp3) {
    assert(0 < a1);
    assert(a1 < nb_registers);

    assert(0 < a2);
    assert(a2 < nb_registers);

    assert(0 < b1);
    assert(b1 < nb_registers);

    assert(0 < b2);
    assert(b2 < nb_registers);

    assert(0 < tmp1);
    assert(tmp1 < nb_registers);

    assert(0 < tmp2);
    assert(tmp2 < nb_registers);

    assert(0 < tmp3);
    assert(tmp3 < nb_registers);

    ASSERT_7_DIFFERENT(a2, a1, b2, b1, tmp1, tmp2, tmp3);

    // b1:a1 <-- a1 + b1
    x_addc(a1, b1, tmp1, tmp2, tmp3);

    // a2 <-- a2 + b2 + carry
    i_add(a2, a2, b2);
    i_add(a2, a2, b1);
  }


  void
  x_addc(unsigned int a, unsigned int b,
         unsigned int tmp1, unsigned int tmp2, unsigned int tmp3) {
    assert(0 < a);
    assert(a < nb_registers);

    assert(0 < b);
    assert(b < nb_registers);

    assert(0 < tmp1);
    assert(tmp1 < nb_registers);

    assert(0 < tmp2);
    assert(tmp2 < nb_registers);

    assert(0 < tmp3);
    assert(tmp3 < nb_registers);

    ASSERT_5_DIFFERENT(a, b, tmp1, tmp2, tmp3);

    x_set0x8000(tmp3);  // tmp3 <-- mask 0b10...0

    i_nand(tmp1, a, tmp3);  // tmp1 <-- 0b11...1 or 0b01...1 if (MSB of first operand) == 1
    i_nand(tmp2, b, tmp3);  // tmp2 <-- 0b11...1 or 0b01...1 if (MSB of second operand) == 1

    // Set sum on 16 bits
    i_add(a, a, b);  // a <-- (first operand) + (second operand)

    i_nand(b, a, tmp3);  // b <-- 0b11...1 or 0b01...1 if (MSB of sum) == 1

    i_beq(tmp1, tmp2, addc_same_tmp1_tmp2);
    i_nand(tmp3, b, tmp3);  // tmp3 <-- 0b11...1 or 0b01...1 if carry on *15* bits sum
  addc_same_tmp1_tmp2:

    // Set carry (if none pair of two MSBs 1 then jump to end)
    i_nand(b, tmp1, tmp2);  // 0b10...0 or 0 if MSBs of tmp1 and tmp2 are 1
    i_beq(b, 0, addc_end);
    i_nand(b, tmp1, tmp3);  // 0b10...0 or 0 if MSBs of tmp1 and tmp3 are 1
    i_beq(b, 0, addc_end);
    i_nand(b, tmp2, tmp3);  // 0b10...0 or 0 if MSBs of tmp2 and tmp3 are 1
    i_beq(b, 0, addc_end);

    i_addi(b, 0, 0x1);  // b <-- carry

  addc_end:
    return;
  }


  void
  x_and_to(unsigned int result, unsigned int a, unsigned int b) {
    assert(0 < result);
    assert(result < nb_registers);

    assert(0 < a);
    assert(a < nb_registers);

    assert(0 < b);
    assert(b < nb_registers);

    i_nand(result, a, b);
    x_not_to(result, result);
  }


  void
  x_inc32(unsigned int a2, unsigned int a1) {
    assert(0 < a2);
    assert(a2 < nb_registers);

    assert(0 < a1);
    assert(a1 < nb_registers);

    assert(a2 != a1);

    i_addi(a1, a1, 0x1);  // a1 <-- a1 + 1
    i_beq(a1, 0, inc32_carry);
    x_branch(inc32_end);
  inc32_carry:
    i_addi(a2, a2, 0x1);  // a2 <-- a2 + carry
  inc32_end:
    return;
  }


  void
  x_is_lower_to(unsigned int result, unsigned int a, unsigned int b,
                unsigned int tmp) {
    assert(0 < result);
    assert(result < nb_registers);

    assert(0 < a);
    assert(a < nb_registers);

    assert(0 < b);
    assert(b < nb_registers);

    assert(0 < tmp);
    assert(tmp < nb_registers);

    ASSERT_4_DIFFERENT(result, a, b, tmp);

    x_mask0x8000_to(result, a);
    i_beq(result, 0, a_msb_0);

    x_mask0x8000_to(result, b);
    i_beq(result, 0, a_msb_1_b_msb_0);
    // a MSB 1, b MSB 1
    x_branch(must_check);

  a_msb_1_b_msb_0:  // a > b
    x_set0(result);
    x_branch(end);

  a_msb_0:
    x_mask0x8000_to(result, b);
    i_beq(result, 0, must_check);  // if a MSB 0, b MSB 0
    // a MSB 0, b MSB 1: a < b
    x_mov(result, 0x1);
    x_branch(end);

  must_check:
    // MSB of a and b are equal
    x_sub_to(result, a, b);
    x_mask0x8000(result, tmp);
  end:
    return;
  }


  void
  x_lshift(unsigned int a) {
    assert(0 < a);
    assert(a < nb_registers);

    i_add(a, a, a);
  }


  void
  x_lshift_to(unsigned int result, unsigned int a) {
    assert(0 < result);
    assert(result < nb_registers);

    assert(0 < a);
    assert(a < nb_registers);

    i_add(result, a, a);
  }


  void
  x_lshift32(unsigned int a2, unsigned int a1,
             unsigned int tmp) {
    assert(0 < a2);
    assert(a2 < nb_registers);

    assert(0 < a1);
    assert(a1 < nb_registers);

    assert(0 < tmp);
    assert(tmp < nb_registers);

    ASSERT_3_DIFFERENT(a2, a1, tmp);

    // tmp <-- 0 or 0b10...0 if MSB of %a1 == 0 or 1
    x_set0x8000(tmp);
    x_and_to(tmp, a1, tmp);

    x_lshift(a1);
    x_lshift(a2);

    // LSB of a2 <-- a2 or (a2 + 1) if MSB of original a1 == 0 or 1
    i_beq(tmp, 0, lshift32_end);
    i_addi(a2, a2, 0x1);
  lshift32_end:
    return;
  }


  void
  x_lshift_8(unsigned int a) {
    assert(0 < a);
    assert(a < nb_registers);

    x_lshift(a);
    x_lshift(a);

    x_lshift(a);
    x_lshift(a);

    x_lshift(a);
    x_lshift(a);

    x_lshift(a);
    x_lshift(a);
  }


  void
  x_lshift_8_to(unsigned int result, unsigned int a) {
    assert(0 < result);
    assert(result < nb_registers);

    assert(0 < a);
    assert(a < nb_registers);

    x_lshift_to(result, a);
    x_lshift(result);

    x_lshift(result);
    x_lshift(result);

    x_lshift(result);
    x_lshift(result);

    x_lshift(result);
    x_lshift(result);
  }


  void
  x_lshift32_8(unsigned int a2, unsigned int a1,
               unsigned int tmp1, unsigned int tmp2, unsigned int tmp3) {
    assert(0 < a2);
    assert(a2 < nb_registers);

    assert(0 < a1);
    assert(a1 < nb_registers);

    assert(0 < tmp1);
    assert(tmp1 < nb_registers);

    assert(0 < tmp2);
    assert(tmp2 < nb_registers);

    assert(0 < tmp3);
    assert(tmp3 < nb_registers);

    ASSERT_5_DIFFERENT(a2, a1, tmp1, tmp2, tmp3);

    x_lshift_8(a2);

    x_rshift_8_to(tmp1, a1, tmp2, tmp3);
    i_add(a2, a2, tmp1);

    x_lshift_8(a1);
  }


  void
  x_mask0x8000(unsigned int a,
               unsigned int tmp) {
    assert(0 < a);
    assert(a < nb_registers);

    assert(0 < tmp);
    assert(tmp < nb_registers);

    assert(a != tmp);

    x_set0x8000(tmp);
    x_and_to(a, a, tmp);
  }


  void
  x_mask0x8000_to(unsigned int result, unsigned int a) {
    assert(0 < result);
    assert(result < nb_registers);

    assert(0 < a);
    assert(a < nb_registers);

    assert(result != a);

    x_set0x8000(result);
    x_and_to(result, a, result);
  }


  void
  x_mov(unsigned int result, unsigned int a) {
    assert(0 < result);
    assert(result < nb_registers);

    assert(0 < a);
    assert(a < nb_registers);

    i_add(result, a, 0);
  }


  void
  x_mul(unsigned int a, unsigned int b,
        unsigned int tmp1, unsigned int tmp2, unsigned int tmp3,
        unsigned int tmp4, unsigned int tmp5) {
    assert(0 < a);
    assert(a < nb_registers);

    assert(0 < b);
    assert(b < nb_registers);

    assert(0 < tmp1);
    assert(tmp1 < nb_registers);

    assert(0 < tmp2);
    assert(tmp2 < nb_registers);

    assert(0 < tmp3);
    assert(tmp3 < nb_registers);

    assert(0 < tmp4);
    assert(tmp4 < nb_registers);

    assert(0 < tmp5);
    assert(tmp5 < nb_registers);

    ASSERT_7_DIFFERENT(a, b, tmp1, tmp2, tmp3, tmp4, tmp5);

    // tmp2:tmp1 will contain the product
    x_set0(tmp1);
    x_set0(tmp2);


    // If first or second operand == 0 then jump to mul_end
    i_beq(a, 0, mul_end);
    i_beq(b, 0, mul_end);


    // Memory 0: first operand (least significant bits)
    //        1: first operand (most significant bits)
    //        2: second operand
    //        3: mask


    // tmp3 <-- second operand (b), and save
    x_mov(tmp3, b);
    i_sw(tmp3, 0, 0x2);  // save second operand


    // b:a will contain first operand on 32 bits, shifted on each step
    x_set0(b);


    // tmp5 <-- mask = 1
    i_addi(tmp5, 0, 0x1);


    // For each bit on the second operand,
    // if the current bit is 1
    // then add the shifted first operand to the partial product.
  mul_loop:
    // INV: b:a == shifted first operand
    //      tmp2:tmp1 == partial product (sum of some shifted first operand)
    //      tmp3 == second operand
    //      tmp5 == mask

    // (Bit of second operand) == 0 or 1 ?
    x_and_to(tmp4, tmp3, tmp5);
    i_beq(tmp4, 0, mul_endif_bit);

    // If (bit of second operand) == 1
    i_sw(a, 0, 0x0);     // save first operand
    i_sw(b, 0, 0x1);
    i_sw(tmp5, 0, 0x3);  // save mask

    // tmp2:tmp1 <-- tmp2:tmp1 + b:a
    x_add32(tmp2, tmp1, b, a, tmp4, tmp3, tmp5);

    i_lw(tmp3, 0, 0x2);  // reload second operand
    i_lw(a, 0, 0x0);     // reload first operand
    i_lw(b, 0, 0x1);
    i_lw(tmp5, 0, 0x3);  // reload mask

  mul_endif_bit:

    // mask <-- mask << 1
    x_lshift(tmp5);
    i_beq(tmp5, 0, mul_end);

    // first operand <-- (first operand) << 1
    x_lshift32(b, a, tmp4);

    // Jump to mul_loop
    i_beq(0, 0, mul_loop);

  mul_end:
    x_mov(a, tmp1);
    x_mov(b, tmp2);
  }


  void
  x_mul_karatsuba(unsigned int a, unsigned int b,
                  unsigned int tmp1, unsigned int tmp2, unsigned int tmp3,
                  unsigned int tmp4, unsigned int tmp5) {
    assert(0 < a);
    assert(a < nb_registers);

    assert(0 < b);
    assert(b < nb_registers);

    assert(0 < tmp1);
    assert(tmp1 < nb_registers);

    assert(0 < tmp2);
    assert(tmp2 < nb_registers);

    assert(0 < tmp3);
    assert(tmp3 < nb_registers);

    assert(0 < tmp4);
    assert(tmp4 < nb_registers);

    assert(0 < tmp5);
    assert(tmp5 < nb_registers);

    ASSERT_7_DIFFERENT(a, b, tmp1, tmp2, tmp3, tmp4, tmp5);

    // Split a and b in 8 bits parts: a2:a1 and b2:b1 and calculates:
    // a2:a1 * b2:b1 == (a2*b2 << 16) + ((a1*b2 + a2*b1) << 8) + a1*b1
    //
    // Calculates a1*b2 + a2*b1 by Karatsuba formula:
    // https://en.wikipedia.org/wiki/Karatsuba_algorithm
    // a1*b2 + a2*b1 == a1*b1 + a2*b2 - (a1 - a2)*(b1 - b2)

    // Memory 0: a1
    //        1: a2
    //        2: b1
    //        3: b2
    //        4: sign of a1 - a2 (0 if >= 0, 0x8000 else)
    //        5: sign of b1 - b2 (0 if >= 0, 0x8000 else)

    // tmp3 <-- a2 and save
    // tmp4 <-- b2 and save
    x_rshift_8_duo_to(tmp3, a, tmp4, b, tmp1, tmp2, tmp5);
    i_sw(tmp3, 0, 0x1);
    i_sw(tmp4, 0, 0x3);

    // tmp1 <-- a1 and save
    // tmp2 <-- b1 and save
    p_movi(tmp5, 0xFF);
    x_and_to(tmp1, a, tmp5);
    x_and_to(tmp2, b, tmp5);
    i_sw(tmp1, 0, 0x0);
    i_sw(tmp2, 0, 0x2);

    // a <-- a1*b1
    x_mul8_to(a, tmp1, tmp2, tmp4, tmp5);

    // b <-- a2*b2
    i_lw(tmp4, 0, 0x3);  // reload b2
    x_mul8_to(b, tmp3, tmp4, tmp1, tmp5);

    // tmp2:tmp1 <-- a1*b1 + a2*b2
    x_mov(tmp1, a);
    x_mov(tmp2, b);
    x_addc(tmp1, tmp2, tmp3, tmp4, tmp5);

    // b:a <-- (a2 *b2):(a1 * b1) + (a1*b1 + a2*b2) << 8
    i_beq(tmp2, 0, karatsuba_not_carry);
    i_lui(tmp5, 0x4);  // tmp5 <-- (1 << 8)
    i_add(b, b, tmp5);
  karatsuba_not_carry:

    x_rshift_8_to(tmp2, tmp1, tmp4, tmp5);
    x_lshift_8(tmp1);

    x_add32(b, a, tmp2, tmp1, tmp3, tmp4, tmp5);

    // tmp2 <-- |a1 - a2| (result on size at most 8 bits)
    i_lw(tmp1, 0, 0x0);  // reload a1
    i_lw(tmp2, 0, 0x1);  // reload a2
    x_sub_from(tmp2, tmp1);  // tmp2 <-- a1 - a2
    x_mask0x8000_to(tmp1, tmp2);
    i_sw(tmp1, 0, 0x4);  // save sign
    i_beq(tmp1, 0, karatsuba_diffa_pos);
    x_neg(tmp2);  // tmp2 <-- a2 - a1
  karatsuba_diffa_pos:

    // tmp4 <-- |b1 - b2| (result on size at most 8 bits)
    i_lw(tmp3, 0, 0x2);  // reload b1
    i_lw(tmp4, 0, 0x3);  // reload b2
    x_sub_from(tmp4, tmp3);  // tmp4 <-- b1 - b2
    x_mask0x8000_to(tmp3, tmp4);
    i_sw(tmp3, 0, 0x5);  // save sign
    i_beq(tmp3, 0, karatsuba_diffb_pos);
    x_neg(tmp4);  // tmp2 <-- a2 - a1
  karatsuba_diffb_pos:

    // tmp1 <-- |a1 - a2| * |b1 - b2|
    x_mul8_to(tmp1, tmp2, tmp4, tmp3, tmp5);

    // tmp2:tmp1 <-- (|a1 - a2| * |b1 - b2|) << 8
    x_rshift_8_to(tmp2, tmp1, tmp4, tmp5);
    x_lshift_8(tmp1);

    // Load and compare sign of differences (a1 - a2) and (b1 - b2)
    i_lw(tmp4, 0, 0x4);
    i_lw(tmp5, 0, 0x5);
    i_beq(tmp4, tmp5, karatsuba_diff_same_sign);
    x_branch(karatsuba_end);
  karatsuba_diff_same_sign:
    // tmp2:tmp1 <-- - tmp2:tmp1
    x_not(tmp2);
    x_not(tmp1);
    x_inc32(tmp2, tmp1);

  karatsuba_end:
    // b:a <-- (a2 *b2):(a1 * b1) + (a1*b1 + a2*b2 - (a1 - a2)*(b1 - b2)) << 8
    x_add32(b, a, tmp2, tmp1, tmp3, tmp4, tmp5);

    return;
  }


  void
  x_mul8_to(unsigned int result, unsigned int a, unsigned int b,
            unsigned int tmp1, unsigned int tmp2) {
    assert(0 < result);
    assert(result < nb_registers);

    assert(0 < a);
    assert(a < nb_registers);
    assert(registers[a] <= 0xFF);

    assert(0 < b);
    assert(b < nb_registers);
    assert(registers[b] <= 0xFF);

    assert(0 < tmp1);
    assert(tmp1 < nb_registers);

    assert(0 < tmp2);
    assert(tmp2 < nb_registers);

    ASSERT_5_DIFFERENT(result, a, b, tmp1, tmp2);

    x_set0(result);
    i_addi(tmp2, 0, 0x1);  // mask <-- 1

  mul8_loop:
    x_and_to(tmp1, b, tmp2);
    i_beq(tmp1, 0, mul8_not1);  // if (current bit of b) == 1
    i_add(result, result, a);
  mul8_not1:

    x_lshift(a);     // a <-- a << 1
    x_lshift(tmp2);  // mask <-- mask << 1

    // If mask <= 0x80 (== 0b10000000 == 128) then loop
    i_lui(tmp1, 0x4);  // tmp1 <-- 0x100 == 0b100000000 == 256
    i_beq(tmp2, tmp1, mul8_end);
    x_branch(mul8_loop);
  mul8_end:
    return;
  }


  void
  x_neg(unsigned int a) {
    assert(0 < a);
    assert(a < nb_registers);

    x_not(a);
    i_addi(a, a, 0x1);
  }


  void
  x_not(unsigned int a) {
    assert(0 < a);
    assert(a < nb_registers);

    i_nand(a, a, a);
  }


  void
  x_not_to(unsigned int result, unsigned int a) {
    assert(0 < result);
    assert(result < nb_registers);

    assert(0 < a);
    assert(a < nb_registers);

    i_nand(result, a, a);
  }


  void
  x_or_to(unsigned int result, unsigned int a, unsigned int b) {
    assert(result < nb_registers);

    assert(0 < a);
    assert(a < nb_registers);

    assert(0 < b);
    assert(b < nb_registers);

    assert(a != b);

    x_not_to(a, a);
    x_not_to(b, b);
    i_nand(result, a, b);
  }


  void
  x_rshift_to(unsigned int result, unsigned int a,
              unsigned int tmp1, unsigned int tmp2) {
    assert(0 < result);
    assert(result < nb_registers);

    assert(0 < a);
    assert(a < nb_registers);

    assert(0 < tmp1);
    assert(tmp1 < nb_registers);

    assert(0 < tmp2);
    assert(tmp2 < nb_registers);

    ASSERT_4_DIFFERENT(result, a, tmp1, tmp2);

    x_set0(result);
    i_addi(tmp1, 0, 0x1);  // target mask

  rshift_loop:
    x_lshift_to(tmp2, tmp1);

    // Test source bit
    x_and_to(tmp2, a, tmp2);
    i_beq(tmp2, 0, rshift_bit0);

    // If source bit was 1 then set target bit to 1
    i_add(result, result, tmp1);
  rshift_bit0:

    x_lshift(tmp1);

    i_beq(tmp1, 0, rshift_end);
    x_branch(rshift_loop);

  rshift_end:
    return;
  }


  void
  x_rshift_8_to(unsigned int result, unsigned int a,
                unsigned int tmp1, unsigned int tmp2) {
    assert(0 < result);
    assert(result < nb_registers);

    assert(0 < a);
    assert(a < nb_registers);

    assert(0 < tmp1);
    assert(tmp1 < nb_registers);

    assert(0 < tmp2);
    assert(tmp2 < nb_registers);

    ASSERT_4_DIFFERENT(result, a, tmp1, tmp2);

    x_set0(result);


    // 5th figure in base 4
    i_lui(tmp1, 0xC);  // source mask = 0x300

    x_and_to(tmp1, a, tmp1);  // tmp1 <-- source & mask
    i_beq(tmp1, 0, rshift_8_5th_end);

    i_lui(tmp2, 0x4);  // 0x100
    i_beq(tmp1, tmp2, rshift_8_5th_masked_1);

    i_lui(tmp2, 0x8);  // 0x200
    i_beq(tmp1, tmp2, rshift_8_5th_masked_2);

    i_addi(result, result, 0x3);  // if masked 3
    x_branch(rshift_8_5th_end);

  rshift_8_5th_masked_2:
    i_addi(result, result, 0x2);
    x_branch(rshift_8_5th_end);

  rshift_8_5th_masked_1:
    i_addi(result, result, 0x1);

  rshift_8_5th_end:


    // 6th figure in base 4
    i_lui(tmp1, 0x30);  // source mask = 0xC00

    x_and_to(tmp1, a, tmp1);  // tmp1 <-- source & mask
    i_beq(tmp1, 0, rshift_8_6th_end);

    i_lui(tmp2, 0x10);  // 0x400
    i_beq(tmp1, tmp2, rshift_8_6th_masked_1);

    i_lui(tmp2, 0x20);  // 0x800
    i_beq(tmp1, tmp2, rshift_8_6th_masked_2);

    i_addi(result, result, 0xC);  // if masked 3
    x_branch(rshift_8_6th_end);

  rshift_8_6th_masked_2:
    i_addi(result, result, 0x8);
    x_branch(rshift_8_6th_end);

  rshift_8_6th_masked_1:
    i_addi(result, result, 0x4);

  rshift_8_6th_end:


    // 7th figure in base 4
    i_lui(tmp1, 0xC0);  // source mask = 0x3000

    x_and_to(tmp1, a, tmp1);  // tmp1 <-- source & mask
    i_beq(tmp1, 0, rshift_8_7th_end);

    i_lui(tmp2, 0x40);  // 0x1000
    i_beq(tmp1, tmp2, rshift_8_7th_masked_1);

    i_lui(tmp2, 0x80);  // 0x2000
    i_beq(tmp1, tmp2, rshift_8_7th_masked_2);

    i_addi(result, result, 0x30);  // if masked 3
    x_branch(rshift_8_7th_end);

  rshift_8_7th_masked_2:
    i_addi(result, result, 0x20);
    x_branch(rshift_8_7th_end);

  rshift_8_7th_masked_1:
    i_addi(result, result, 0x10);

  rshift_8_7th_end:


    // 8th figure in base 4
    i_lui(tmp1, 0x300);  // source mask = 0xC000

    x_and_to(tmp1, a, tmp1);  // tmp1 <-- source & mask
    i_beq(tmp1, 0, rshift_8_8th_end);

    i_lui(tmp2, 0x100);  // 0x4000
    i_beq(tmp1, tmp2, rshift_8_8th_masked_1);

    i_lui(tmp2, 0x200);  // 0x8000
    i_beq(tmp1, tmp2, rshift_8_8th_masked_2);

    i_lui(tmp1, 0x3);  // 0xC0
    i_add(result, result, tmp1);  // if masked 3
    x_branch(rshift_8_8th_end);

  rshift_8_8th_masked_2:
    i_lui(tmp1, 0x2);  // 0x80
    i_add(result, result, tmp1);
    x_branch(rshift_8_8th_end);

  rshift_8_8th_masked_1:
    i_lui(tmp1, 0x1);  // 0x40
    i_add(result, result, tmp1);

  rshift_8_8th_end:
    return;
  }


  void
  x_rshift_8_duo_to(unsigned int result_a, unsigned int a,
                    unsigned int result_b, unsigned int b,
                    unsigned int tmp1, unsigned int tmp2, unsigned int tmp3) {
    assert(0 < result_a);
    assert(result_a < nb_registers);

    assert(0 < a);
    assert(a < nb_registers);

    assert(0 < result_b);
    assert(result_b < nb_registers);

    assert(0 < b);
    assert(b < nb_registers);

    assert(0 < tmp1);
    assert(tmp1 < nb_registers);

    assert(0 < tmp2);
    assert(tmp2 < nb_registers);

    assert(0 < tmp3);
    assert(tmp3 < nb_registers);

    ASSERT_7_DIFFERENT(result_a, a, result_b, b, tmp1, tmp2, tmp3);

    x_set0(result_a);
    x_set0(result_b);


    i_lui(tmp3, 0xC);  // source mask = 0x300

    // 5th figure of a in base 4
    x_and_to(tmp1, a, tmp3);  // tmp1 <-- source & mask
    i_beq(tmp1, 0, rshift_8_a_5th_end);

    i_lui(tmp2, 0x4);  // 0x100
    i_beq(tmp1, tmp2, rshift_8_a_5th_masked_1);

    i_lui(tmp2, 0x8);  // 0x200
    i_beq(tmp1, tmp2, rshift_8_a_5th_masked_2);

    i_addi(result_a, result_a, 0x3);  // if masked 3
    x_branch(rshift_8_a_5th_end);

  rshift_8_a_5th_masked_2:
    i_addi(result_a, result_a, 0x2);
    x_branch(rshift_8_a_5th_end);

  rshift_8_a_5th_masked_1:
    i_addi(result_a, result_a, 0x1);

  rshift_8_a_5th_end:


    // 5th figure of b in base 4
    x_and_to(tmp1, b, tmp3);  // tmp1 <-- source & mask
    i_beq(tmp1, 0, rshift_8_b_5th_end);

    i_lui(tmp2, 0x4);  // 0x100
    i_beq(tmp1, tmp2, rshift_8_b_5th_masked_1);

    i_lui(tmp2, 0x8);  // 0x200
    i_beq(tmp1, tmp2, rshift_8_b_5th_masked_2);

    i_addi(result_b, result_b, 0x3);  // if masked 3
    x_branch(rshift_8_b_5th_end);

  rshift_8_b_5th_masked_2:
    i_addi(result_b, result_b, 0x2);
    x_branch(rshift_8_b_5th_end);

  rshift_8_b_5th_masked_1:
    i_addi(result_b, result_b, 0x1);

  rshift_8_b_5th_end:


    i_lui(tmp3, 0x30);  // source mask = 0xC00

    // 6th figure of a in base 4
    x_and_to(tmp1, a, tmp3);  // tmp1 <-- source & mask
    i_beq(tmp1, 0, rshift_8_a_6th_end);

    i_lui(tmp2, 0x10);  // 0x400
    i_beq(tmp1, tmp2, rshift_8_a_6th_masked_1);

    i_lui(tmp2, 0x20);  // 0x800
    i_beq(tmp1, tmp2, rshift_8_a_6th_masked_2);

    i_addi(result_a, result_a, 0xC);  // if masked 3
    x_branch(rshift_8_a_6th_end);

  rshift_8_a_6th_masked_2:
    i_addi(result_a, result_a, 0x8);
    x_branch(rshift_8_a_6th_end);

  rshift_8_a_6th_masked_1:
    i_addi(result_a, result_a, 0x4);

  rshift_8_a_6th_end:


    // 6th figure of b in base 4
    x_and_to(tmp1, b, tmp3);  // tmp1 <-- source & mask
    i_beq(tmp1, 0, rshift_8_b_6th_end);

    i_lui(tmp2, 0x10);  // 0x400
    i_beq(tmp1, tmp2, rshift_8_b_6th_masked_1);

    i_lui(tmp2, 0x20);  // 0x800
    i_beq(tmp1, tmp2, rshift_8_b_6th_masked_2);

    i_addi(result_b, result_b, 0xC);  // if masked 3
    x_branch(rshift_8_b_6th_end);

  rshift_8_b_6th_masked_2:
    i_addi(result_b, result_b, 0x8);
    x_branch(rshift_8_b_6th_end);

  rshift_8_b_6th_masked_1:
    i_addi(result_b, result_b, 0x4);

  rshift_8_b_6th_end:


    i_lui(tmp3, 0xC0);  // source mask = 0x3000

    // 7th figure of a in base 4
    x_and_to(tmp1, a, tmp3);  // tmp1 <-- source & mask
    i_beq(tmp1, 0, rshift_8_a_7th_end);

    i_lui(tmp2, 0x40);  // 0x1000
    i_beq(tmp1, tmp2, rshift_8_a_7th_masked_1);

    i_lui(tmp2, 0x80);  // 0x2000
    i_beq(tmp1, tmp2, rshift_8_a_7th_masked_2);

    i_addi(result_a, result_a, 0x30);  // if masked 3
    x_branch(rshift_8_a_7th_end);

  rshift_8_a_7th_masked_2:
    i_addi(result_a, result_a, 0x20);
    x_branch(rshift_8_a_7th_end);

  rshift_8_a_7th_masked_1:
    i_addi(result_a, result_a, 0x10);

  rshift_8_a_7th_end:


    // 7th figure of b in base 4
    x_and_to(tmp1, b, tmp3);  // tmp1 <-- source & mask
    i_beq(tmp1, 0, rshift_8_b_7th_end);

    i_lui(tmp2, 0x40);  // 0x1000
    i_beq(tmp1, tmp2, rshift_8_b_7th_masked_1);

    i_lui(tmp2, 0x80);  // 0x2000
    i_beq(tmp1, tmp2, rshift_8_b_7th_masked_2);

    i_addi(result_b, result_b, 0x30);  // if masked 3
    x_branch(rshift_8_b_7th_end);

  rshift_8_b_7th_masked_2:
    i_addi(result_b, result_b, 0x20);
    x_branch(rshift_8_b_7th_end);

  rshift_8_b_7th_masked_1:
    i_addi(result_b, result_b, 0x10);

  rshift_8_b_7th_end:


    i_lui(tmp3, 0x300);  // source mask = 0xC000

    // 8th figure of a in base 4
    x_and_to(tmp1, a, tmp3);  // tmp1 <-- source & mask
    i_beq(tmp1, 0, rshift_8_a_8th_end);

    i_lui(tmp2, 0x100);  // 0x4000
    i_beq(tmp1, tmp2, rshift_8_a_8th_masked_1);

    i_lui(tmp2, 0x200);  // 0x8000
    i_beq(tmp1, tmp2, rshift_8_a_8th_masked_2);

    i_lui(tmp1, 0x3);  // 0xC0
    i_add(result_a, result_a, tmp1);  // if masked 3
    x_branch(rshift_8_a_8th_end);

  rshift_8_a_8th_masked_2:
    i_lui(tmp1, 0x2);  // 0x80
    i_add(result_a, result_a, tmp1);
    x_branch(rshift_8_a_8th_end);

  rshift_8_a_8th_masked_1:
    i_lui(tmp1, 0x1);  // 0x40
    i_add(result_a, result_a, tmp1);

  rshift_8_a_8th_end:


    // 8th figure of b in base 4
    x_and_to(tmp1, b, tmp3);  // tmp1 <-- source & mask
    i_beq(tmp1, 0, rshift_8_b_8th_end);

    i_lui(tmp2, 0x100);  // 0x4000
    i_beq(tmp1, tmp2, rshift_8_b_8th_masked_1);

    i_lui(tmp2, 0x200);  // 0x8000
    i_beq(tmp1, tmp2, rshift_8_b_8th_masked_2);

    i_lui(tmp1, 0x3);  // 0xC0
    i_add(result_b, result_b, tmp1);  // if masked 3
    x_branch(rshift_8_b_8th_end);

  rshift_8_b_8th_masked_2:
    i_lui(tmp1, 0x2);  // 0x80
    i_add(result_b, result_b, tmp1);
    x_branch(rshift_8_b_8th_end);

  rshift_8_b_8th_masked_1:
    i_lui(tmp1, 0x1);  // 0x40
    i_add(result_b, result_b, tmp1);

  rshift_8_b_8th_end:
    return;
  }


  void
  x_rshift_8_signed_to(unsigned int result, unsigned int a,
                       unsigned int tmp1, unsigned int tmp2,
                       unsigned int tmp3) {
    assert(0 < result);
    assert(result < nb_registers);

    assert(0 < a);
    assert(a < nb_registers);

    assert(0 < tmp1);
    assert(tmp1 < nb_registers);

    assert(0 < tmp2);
    assert(tmp2 < nb_registers);

    assert(0 < tmp3);
    assert(tmp3 < nb_registers);

    ASSERT_5_DIFFERENT(result, a, tmp1, tmp2, tmp3);

    x_mask0x8000_to(tmp3, a);

    x_rshift_8_to(result, a, tmp1, tmp2);

    i_beq(tmp3, 0, rshift_8_signed_end);
    i_lui(tmp1, 0x3FC);  // tmp1 <-- 0xFF00
    i_add(result, result, tmp1);
  rshift_8_signed_end:
    return;
  }


  void
  x_set0(unsigned int result) {
    assert(0 < result);
    assert(result < nb_registers);

    i_add(result, 0, 0);
  }


  void
  x_set0x8000(unsigned int a) {
    assert(0 < a);
    assert(a < nb_registers);

    i_lui(a, 0x200);  // = 0x200 << 6
  }


  void
  x_sqr(unsigned int a, unsigned int result2,
        unsigned int tmp1, unsigned int tmp2, unsigned int tmp3,
        unsigned int tmp4, unsigned int tmp5) {
    assert(0 < a);
    assert(a < nb_registers);

    assert(0 < result2);
    assert(result2 < nb_registers);

    assert(0 < tmp1);
    assert(tmp1 < nb_registers);

    assert(0 < tmp2);
    assert(tmp2 < nb_registers);

    assert(0 < tmp3);
    assert(tmp3 < nb_registers);

    assert(0 < tmp4);
    assert(tmp4 < nb_registers);

    assert(0 < tmp5);
    assert(tmp5 < nb_registers);

    ASSERT_7_DIFFERENT(a, result2, tmp1, tmp2, tmp3, tmp4, tmp5);

    // Split a in 8 bits parts: a2:a1 and calculates:
    // a2:a1 * a2:a1 == (a2*a2 << 16) + (a1*a2 << 9) + a1*a1

    // Memory 0: a1
    //        1: a2

    // tmp2 <-- a2 and save
    x_rshift_8_to(tmp2, a, tmp1, tmp3);
    i_sw(tmp2, 0, 0x1);

    // tmp1 <-- a1 and save
    p_movi(tmp3, 0xFF);
    x_and_to(tmp1, a, tmp3);
    i_sw(tmp1, 0, 0x0);

    // a <-- a1*a1
    x_sqr8_to(a, tmp1, tmp3, tmp4, tmp5);

    // result2 <-- a2*a2
    x_sqr8_to(result2, tmp2, tmp3, tmp4, tmp5);

    // tmp1 <-- a1 * a2
    i_lw(tmp2, 0, 0x0);  // reload a1
    i_lw(tmp3, 0, 0x1);  // reload a2
    x_mul8_to(tmp1, tmp2, tmp3, tmp4, tmp5);

    // tmp2:tmp1 <-- (a1 * a2) << 8
    x_rshift_8_to(tmp2, tmp1, tmp4, tmp5);
    x_lshift_8(tmp1);

    // tmp2:tmp1 <-- (a1 * a2) << 9
    x_lshift32(tmp2, tmp1, tmp3);

    // result2:a <-- (a2 *a2):(a1 * a1) + (a1 * a2) << 9
    x_add32(result2, a, tmp2, tmp1, tmp3, tmp4, tmp5);

    return;
  }


  void
  x_sqr8_to(unsigned int result, unsigned int a,
            unsigned int tmp1, unsigned int tmp2, unsigned int tmp3) {
    assert(0 < result);
    assert(result < nb_registers);

    assert(0 < a);
    assert(a < nb_registers);
    assert(registers[a] <= 0xFF);

    assert(0 < tmp1);
    assert(tmp1 < nb_registers);

    assert(0 < tmp2);
    assert(tmp2 < nb_registers);

    assert(0 < tmp3);
    assert(tmp3 < nb_registers);

    ASSERT_5_DIFFERENT(result, a, tmp1, tmp2, tmp3);

    // TODO(OPi) Specific algo ?

    x_mov(tmp3, a);

    x_set0(result);
    i_addi(tmp2, 0, 0x1);  // mask <-- 1

  sqr8_loop:
    x_and_to(tmp1, tmp3, tmp2);
    i_beq(tmp1, 0, sqr8_not1);  // if (current bit of b) == 1
    i_add(result, result, a);
  sqr8_not1:

    x_lshift(a);     // a <-- a << 1
    x_lshift(tmp2);  // mask <-- mask << 1

    // If mask <= 0x80 (== 0b10000000 == 128) then loop
    i_lui(tmp1, 0x4);  // tmp1 <-- 0x100 == 0b100000000 == 256
    i_beq(tmp2, tmp1, sqr8_end);
    x_branch(sqr8_loop);
  sqr8_end:
    return;
  }


  void
  x_sub_from(unsigned int a, unsigned int b) {
    assert(0 < a);
    assert(a < nb_registers);

    assert(0 < b);
    assert(b < nb_registers);

    assert(a != b);

    x_neg(a);
    i_add(a, a, b);
  }


  void
  x_sub_to(unsigned int result, unsigned int a, unsigned int b) {
    assert(0 < result);
    assert(result < nb_registers);

    assert(0 < a);
    assert(a < nb_registers);

    assert(0 < b);
    assert(b < nb_registers);

    ASSERT_3_DIFFERENT(result, a, b);

    x_mov(result, b);
    x_sub_from(result, a);
  }


  void
  x_swap(unsigned int a, unsigned int b, unsigned int tmp) {
    assert(0 < a);
    assert(a < nb_registers);

    assert(0 < b);
    assert(b < nb_registers);

    assert(0 < tmp);
    assert(tmp < nb_registers);

    ASSERT_3_DIFFERENT(a, b, tmp);

    x_mov(tmp, a);
    x_mov(a, b);
    x_mov(b, tmp);
  }
}  // namespace cpprisc16