/** 
 * ZuluSCSI™ - Copyright (c) 2022 Rabbit Hole Computing™
 * 
 * ZuluSCSI™ firmware is licensed under the GPL version 3 or any later version. 
 * 
 * https://www.gnu.org/licenses/gpl-3.0.html
 * ----
 * 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 <https://www.gnu.org/licenses/>.
**/

#include "scsi_accel_asm.h"
#include "ZuluSCSI_platform.h"

// Optimized ASM blocks for the SCSI communication subroutine

// Take 8 bits from d and format them for writing
// d is name of data operand, b is bit offset, x is unique label
#define ASM_LOAD_DATA(d, b, x) \
"    load_data1_" x "_%=: \n" \
"        ubfx    %[tmp1], %[" d "], #" b ", #8 \n" \
"        ldr     %[tmp1], [%[byte_lookup], %[tmp1], lsl #2] \n"

// Write data to SCSI port and set REQ high
#define ASM_SEND_DATA(x) \
"    send_data" x "_%=: \n" \
"        str     %[tmp1], [%[out_port_bop]] \n"

// Read data from SCSI port, set REQ high, store data to d at bit offset b
#define ASM_RECV_DATA(d, b, x) \
"    recv_data" x "_%=: \n" \
"        ldr     %[tmp1], [%[in_port_istat]] \n" \
"        mov     %[tmp2], %[req_high_bop] \n" \
"        str     %[tmp2], [%[out_port_bop]] \n" \
"        ubfx    %[tmp1], %[tmp1], %[data_in_shift], #8 \n" \
"        bfi     %[" d "], %[tmp1], #" b ", #8 \n"

// Wait for ACK to be high, set REQ low, wait ACK low
#define ASM_HANDSHAKE(x) \
"        ldr     %[tmp2], [%[ack_pin_bb]] \n" \
"        str     %[tmp2], [%[req_pin_bb]] \n" \
"        cbnz    %[tmp2], req_is_low_now" x "_%= \n" \
"        ldr     %[tmp2], [%[ack_pin_bb]] \n" \
"        str     %[tmp2], [%[req_pin_bb]] \n" \
"        cbnz    %[tmp2], req_is_low_now" x "_%= \n" \
"        ldr     %[tmp2], [%[ack_pin_bb]] \n" \
"        str     %[tmp2], [%[req_pin_bb]] \n" \
"        cbnz    %[tmp2], req_is_low_now" x "_%= \n" \
"    wait_ack_inactive" x "_%=: \n" \
"        ldr     %[tmp2], [%[ack_pin_bb]] \n" \
"        str     %[tmp2], [%[req_pin_bb]] \n" \
"        cbnz    %[tmp2], req_is_low_now" x "_%= \n" \
"        ldr     %[tmp2], [%[reset_flag]] \n" \
"        cbnz    %[tmp2], req_is_low_now" x "_%= \n" \
"        b.n     wait_ack_inactive" x "_%= \n" \
"    req_is_low_now" x "_%=: \n" \
"        ldr     %[tmp2], [%[ack_pin_bb]] \n" \
"        cbz     %[tmp2], over_ack_active" x "_%= \n" \
"        ldr     %[tmp2], [%[ack_pin_bb]] \n" \
"        cbz     %[tmp2], over_ack_active" x "_%= \n" \
"        ldr     %[tmp2], [%[ack_pin_bb]] \n" \
"        cbz     %[tmp2], over_ack_active" x "_%= \n" \
"        ldr     %[tmp2], [%[ack_pin_bb]] \n" \
"        cbz     %[tmp2], over_ack_active" x "_%= \n" \
"    wait_ack_active" x "_%=: \n" \
"        ldr     %[tmp2], [%[ack_pin_bb]] \n" \
"        cbz     %[tmp2], over_ack_active" x "_%= \n" \
"        ldr     %[tmp2], [%[reset_flag]] \n" \
"        cbnz    %[tmp2], over_ack_active" x "_%= \n" \
"        b.n     wait_ack_active" x "_%= \n" \
"    over_ack_active" x "_%=: \n" \

// Send bytes to SCSI bus using the asynchronous handshake mechanism
// Takes 4 bytes at a time for sending from buf.
void scsi_accel_asm_send(const uint32_t *buf, uint32_t num_words, volatile int *resetFlag)
{
    volatile uint32_t *out_port_bop = (volatile uint32_t*)&GPIO_BOP(SCSI_OUT_PORT);
    const uint32_t *byte_lookup = g_scsi_out_byte_to_bop;
    uint32_t ack_pin_bb = PERIPH_BB_BASE + (((uint32_t)&GPIO_ISTAT(SCSI_ACK_PORT)) - APB1_BUS_BASE) * 32 + SCSI_IN_ACK_IDX * 4;
    uint32_t req_pin_bb = PERIPH_BB_BASE + (((uint32_t)out_port_bop) - APB1_BUS_BASE) * 32 + (SCSI_OUT_REQ_IDX + 16) * 4;
    register uint32_t tmp1 = 0;
    register uint32_t tmp2 = 0;
    register uint32_t data = 0;

    asm volatile (
    "   ldr      %[data], [%[buf]], #4 \n" \
        ASM_LOAD_DATA("data", "0", "first")

    "inner_loop_%=: \n" \
        ASM_SEND_DATA("0")
        ASM_LOAD_DATA("data", "8", "8")
        ASM_HANDSHAKE("0")
        
        ASM_SEND_DATA("8")
        ASM_LOAD_DATA("data", "16", "16")
        ASM_HANDSHAKE("8")

        ASM_SEND_DATA("16")
        ASM_LOAD_DATA("data", "24", "24")
        ASM_HANDSHAKE("16")

        ASM_SEND_DATA("24")
    "   ldr      %[data], [%[buf]], #4 \n" \
        ASM_LOAD_DATA("data", "0", "0")
        ASM_HANDSHAKE("24")

    "   subs     %[num_words], %[num_words], #1 \n" \
    "   bne     inner_loop_%= \n"
    : /* Output */ [tmp1] "+l" (tmp1), [tmp2] "+l" (tmp2), [data] "+r" (data),
                   [buf] "+r" (buf), [num_words] "+r" (num_words)
    : /* Input */ [ack_pin_bb] "r" (ack_pin_bb),
                  [req_pin_bb] "r" (req_pin_bb),
                  [out_port_bop] "r"(out_port_bop),
                  [byte_lookup] "r" (byte_lookup),
                  [reset_flag] "r" (resetFlag)
    : /* Clobber */ );

    SCSI_RELEASE_DATA_REQ();
}

// Read bytes from SCSI bus using asynchronous handshake mechanism
// Takes 4 bytes at a time.
void scsi_accel_asm_recv(uint32_t *buf, uint32_t num_words, volatile int *resetFlag)
{
    volatile uint32_t *out_port_bop = (volatile uint32_t*)&GPIO_BOP(SCSI_OUT_PORT);
    volatile uint32_t *in_port_istat = (volatile uint32_t*)&GPIO_ISTAT(SCSI_IN_PORT);
    uint32_t ack_pin_bb = PERIPH_BB_BASE + (((uint32_t)&GPIO_ISTAT(SCSI_ACK_PORT)) - APB1_BUS_BASE) * 32 + SCSI_IN_ACK_IDX * 4;
    uint32_t req_pin_bb = PERIPH_BB_BASE + (((uint32_t)out_port_bop) - APB1_BUS_BASE) * 32 + (SCSI_OUT_REQ_IDX + 16) * 4;
    register uint32_t tmp1 = 0;
    register uint32_t tmp2 = 0;
    register uint32_t data = 0;

    asm volatile (
    "inner_loop_%=: \n" \
        ASM_HANDSHAKE("0")
        ASM_RECV_DATA("data", "0", "0")
        
        ASM_HANDSHAKE("8")
        ASM_RECV_DATA("data", "8", "8")
        
        ASM_HANDSHAKE("16")
        ASM_RECV_DATA("data", "16", "16")
        
        ASM_HANDSHAKE("24")
        ASM_RECV_DATA("data", "24", "24")

    "   mvn      %[data], %[data] \n" \
    "   str      %[data], [%[buf]], #4 \n" \
    "   subs     %[num_words], %[num_words], #1 \n" \
    "   bne     inner_loop_%= \n"
    : /* Output */ [tmp1] "+l" (tmp1), [tmp2] "+l" (tmp2), [data] "+r" (data),
                   [buf] "+r" (buf), [num_words] "+r" (num_words)
    : /* Input */ [ack_pin_bb] "r" (ack_pin_bb),
                  [req_pin_bb] "r" (req_pin_bb),
                  [out_port_bop] "r"(out_port_bop),
                  [in_port_istat] "r" (in_port_istat),
                  [reset_flag] "r" (resetFlag),
                  [data_in_shift] "I" (SCSI_IN_SHIFT),
                  [req_high_bop] "I" (SCSI_OUT_REQ)
    : /* Clobber */ );

    SCSI_RELEASE_DATA_REQ();
}