/***************************************************************************
 *             __________               __   ___.
 *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
 *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
 *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
 *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
 *                     \/            \/     \/    \/            \/
 *
 * Copyright (C) 2007 by Jens Arnold
 *
 * 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 2
 * of the License, or (at your option) any later version.
 *
 * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
 * KIND, either express or implied.
 *
 ****************************************************************************/

#include "config.h"

    .syntax unified

#if CONFIG_CPU == PP5002
    /* Causes ATA retries on iPod G3 probably related to improper controller
     * setup. Needs investigation. */
    .section    .icode,"ax",%progbits
    .equ    .ata_port, 0xc00031e0
#elif defined CPU_PP502x
    /* Verified working on (PP5020, PP5022) targets */
    .section    .icode,"ax",%progbits
    .equ    .ata_port, 0xc30001e0
#elif CONFIG_CPU == S3C2440
    /* Untested */
    .text
    .equ    .ata_port, 0x18000000
#elif defined(MROBE_500)
    .text
    .equ    .ata_port, 0x50400000
#else
    /* This isn't vaild, but it does it's job, not sure what this should be */
    #error ata_port undefined!
#endif

    .align  2
    .global copy_read_sectors
    .type   copy_read_sectors,%function
    
/* Read a number of words from the ATA data port
 *
 * Optimised for speed; assumes wordcount >= 10
 *
 * Arguments:
 *   r0 - buffer address
 *   r1 - word count
 *
 * Register usage:
 *   r0 - current address
 *   r1 - word count
 *   r2 - ata port
 *   r3..r5, r12, lr - read buffers
 */

copy_read_sectors:
    stmfd   sp!, {r4, r5, lr}
    ldr     r2, =.ata_port
    tst     r0, #1              /* 16 bit aligned? */
    beq     .r_aligned

    /* not 16-bit aligned */
    sub     r1, r1, #1          /* one halfword is handled unconditionally */
    ldrh    r3, [r2]            /* read first halfword */
    strb    r3, [r0], #1        /* store low byte */
    mov     r3, r3, lsr #8

    tst     r0, #2              /* 32 bit aligned? */
    beq     .r_noword_u
    ldrh    r4, [r2]            /* read second halfword */
    orr     r3, r3, r4, lsl #8  /* combine with old byte */
    strh    r3, [r0], #2        /* store */
    mov     r3, r4, lsr #8
    sub     r1, r1, #1          /* another halfword taken */
.r_noword_u:

    sub     r1, r1, #8          /* adjust for zero-check and doing 8 halfwords/loop */
.r_loop_u:
    ldrh    r4, [r2]            /* Read 8 halfwords and combine them into */
    orr     r3, r3, r4, lsl #8  /*  4 words so that they're properly aligned */
    ldrh    r4, [r2]            /*  in memory. Bottom byte of first word is */
    orr     r3, r3, r4, lsl #24 /*  the top byte from the last round. Write */
    mov     r4, r4, lsr #8      /*  all 4 words at once. */
    ldrh    r5, [r2]
    orr     r4, r4, r5, lsl #8
    ldrh    r5, [r2]
    orr     r4, r4, r5, lsl #24
    mov     r5, r5, lsr #8
    ldrh    r12, [r2]
    orr     r5, r5, r12, lsl #8
    ldrh    r12, [r2]
    orr     r5, r5, r12, lsl #24
    mov     r12, r12, lsr #8
    ldrh    lr, [r2]
    orr     r12, r12, lr, lsl #8
    ldrh    lr, [r2]
    orr     r12, r12, lr, lsl #24
    stmia   r0!, {r3, r4, r5, r12}
    mov     r3, lr, lsr #8
    subs    r1, r1, #8          /* 8 or more halfwords left? */
    bge     .r_loop_u

    /* No need to adjust the count, only checking bits from now on. */
    tst     r1, #4              /* 4 or more halfwords left? */
    beq     .r_end4_u
    ldrh    r4, [r2]
    orr     r3, r3, r4, lsl #8
    ldrh    r4, [r2]
    orr     r3, r3, r4, lsl #24
    mov     r4, r4, lsr #8
    ldrh    r5, [r2]
    orr     r4, r4, r5, lsl #8
    ldrh    r5, [r2]
    orr     r4, r4, r5, lsl #24
    stmia   r0!, {r3, r4}
    mov     r3, r5, lsr #8
.r_end4_u:

    tst     r1, #2              /* 2 or more halfwords left? */
    beq     .r_end2_u
    ldrh    r4, [r2]
    orr     r3, r3, r4, lsl #8
    ldrh    r4, [r2]
    orr     r3, r3, r4, lsl #24
    str     r3, [r0], #4
    mov     r3, r4, lsr #8
.r_end2_u:

    tst     r1, #1              /* one halfword left? */
    ldrhne  r4, [r2]
    orrne   r3, r3, r4, lsl #8
    strhne  r3, [r0], #2
    movne   r3, r4, lsr #8

    strb    r3, [r0], #1        /* store final byte */

    ldmpc   regs=r4-r5

    /* 16-bit aligned */
.r_aligned:
    tst     r0, #2              /* 32 bit aligned? */
    ldrhne  r3, [r2]            /* no: read first halfword */
    strhne  r3, [r0], #2        /*     store */
    subne   r1, r1, #1          /*     one halfword taken */

    sub     r1, r1, #8          /* adjust for zero-check and doing 8 halfwords/loop */
.r_loop_a:
    ldrh    r3, [r2]            /* Read 8 halfwords and combine each pair */
    ldrh    r4, [r2]            /*  into a word, then store all at once. */
    orr     r3, r3, r4, lsl #16
    ldrh    r4, [r2]
    ldrh    r5, [r2]
    orr     r4, r4, r5, lsl #16
    ldrh    r5, [r2]
    ldrh    r12, [r2]
    orr     r5, r5, r12, lsl #16
    ldrh    r12, [r2]
    ldrh    lr, [r2]
    orr     r12, r12, lr, lsl #16
    stmia   r0!, {r3, r4, r5, r12}
    subs    r1, r1, #8          /* 8 or more halfwords left? */
    bge     .r_loop_a
    
    /* No need to adjust the count, only checking bits from now on. */
    tst     r1, #4              /* 4 or more halfwords left? */
    beq     .r_end4_a
    ldrh    r3, [r2]
    ldrh    r4, [r2]
    orr     r3, r3, r4, lsl #16
    ldrh    r4, [r2]
    ldrh    r5, [r2]
    orr     r4, r4, r5, lsl #16
    stmia   r0!, {r3, r4}
.r_end4_a:

    tst     r1, #2              /* 2 or more halfwords left? */
    ldrhne  r3, [r2]
    ldrhne  r4, [r2]
    orrne   r3, r3, r4, lsl #16
    strne   r3, [r0], #4

    tst     r1, #1              /* one halfword left? */
    ldrhne  r3, [r2]
    strhne  r3, [r0], #2

    ldmpc   regs=r4-r5

.r_end:
    .size   copy_read_sectors,.r_end-copy_read_sectors

    .align  2
    .global copy_write_sectors
    .type   copy_write_sectors,%function
    
/* Write a number of words to the ATA data port
 *
 * Optimised for speed; assumes wordcount >= 10
 *
 * Arguments:
 *   r0 - buffer address
 *   r1 - word count
 *
 * Register usage:
 *   r0 - current address
 *   r1 - word count
 *   r2 - ata port
 *   r3..r5, r12, lr - read buffers
 */

copy_write_sectors:
    stmfd   sp!, {r4, r5, lr}
    ldr     r2, =.ata_port
    tst     r0, #1              /* 16 bit aligned? */
    beq     .w_aligned
    
    /* not 16-bit aligned */
    sub     r1, r1, #1          /* one halfword is done unconditionally */
    ldrb    r3, [r0], #1        /* load 1st byte, now halfword aligned. */

    tst     r0, #2              /* 32 bit aligned? */
    beq     .w_noword_u
    ldrh    r4, [r0], #2        /* load a halfword */
    orr     r3, r3, r4, lsl #8  /* combine with old byte */
    strh    r3, [r2]            /* write halfword */
    mov     r3, r4, lsr #8
    sub     r1, r1, #1          /* another halfword taken */
.w_noword_u:

    sub     r1, r1, #8          /* adjust for zero-check and doing 8 halfwords/loop */
.w_loop_u:
    ldmia   r0!, {r4, r5, r12, lr}
    orr     r3, r3, r4, lsl #8  /* Load 4 words at once and decompose them */
    strh    r3, [r2]            /*  into 8 halfwords in a way that the words */
    mov     r3, r3, lsr #16     /*  are shifted by 8 bits, putting the high */
    strh    r3, [r2]            /*  byte of one word into the low byte of */
    mov     r4, r4, lsr #24     /*  the next. High byte of last word becomes */
    orr     r4, r4, r5, lsl #8  /*  low byte of next round. */
    strh    r4, [r2]
    mov     r4, r4, lsr #16
    strh    r4, [r2]
    mov     r5, r5, lsr #24
    orr     r5, r5, r12, lsl #8
    strh    r5, [r2]
    mov     r5, r5, lsr #16
    strh    r5, [r2]
    mov     r12, r12, lsr #24
    orr     r12, r12, lr, lsl #8
    strh    r12, [r2]
    mov     r12, r12, lsr #16
    strh    r12, [r2]
    mov     r3, lr, lsr #24
    subs    r1, r1, #8          /* 8 or more halfwords left? */
    bge     .w_loop_u
    
    /* No need to adjust the count, only checking bits from now on. */
    tst     r1, #4              /* 4 or more halfwords left? */
    beq     .w_end4_u
    ldmia   r0!, {r4, r5}
    orr     r3, r3, r4, lsl #8
    strh    r3, [r2]
    mov     r3, r3, lsr #16
    strh    r3, [r2]
    mov     r4, r4, lsr #24
    orr     r4, r4, r5, lsl #8
    strh    r4, [r2]
    mov     r4, r4, lsr #16
    strh    r4, [r2]
    mov     r3, r5, lsr #24
.w_end4_u:

    tst     r1, #2              /* 2 or more halfwords left? */
    beq     .w_end2_u
    ldr     r4, [r0], #4
    orr     r3, r3, r4, lsl #8
    strh    r3, [r2]
    mov     r3, r3, lsr #16
    strh    r3, [r2]
    mov     r3, r4, lsr #24
.w_end2_u:

    tst     r1, #1              /* one halfword left? */
    ldrhne  r4, [r0], #2
    orrne   r3, r3, r4, lsl #8
    strhne  r3, [r2]
    movne   r3, r3, lsr #16
    
    ldrb    r4, [r0], #1        /* load final byte */
    orr     r3, r3, r4, lsl #8
    strh    r3, [r2]            /* write final halfword */

    ldmpc   regs=r4-r5

    /* 16-bit aligned */
.w_aligned:
    tst     r0, #2              /* 32 bit aligned? */
    ldrhne  r3, [r0], #2        /* no: load first halfword */
    strhne  r3, [r2]            /*     write */
    subne   r1, r1, #1          /*     one halfword taken */

    sub     r1, r1, #8          /* adjust for zero-check and doing 8 halfwords/loop */
.w_loop_a:
    ldmia   r0!, {r3, r4, r5, r12}
    strh    r3, [r2]            /* Load 4 words and decompose them into */
    mov     r3, r3, lsr #16     /*  2 halfwords each, and write those. */
    strh    r3, [r2]
    strh    r4, [r2]
    mov     r4, r4, lsr #16
    strh    r4, [r2]
    strh    r5, [r2]
    mov     r5, r5, lsr #16
    strh    r5, [r2]
    strh    r12, [r2]
    mov     r12, r12, lsr #16
    strh    r12, [r2]
    subs    r1, r1, #8          /* 8 or more halfwords left? */
    bge     .w_loop_a

    /* No need to adjust the count, only checking bits from now on. */
    tst     r1, #4              /* 4 or more halfwords left? */
    beq     .w_end4_a
    ldmia   r0!, {r3, r4}
    strh    r3, [r2]
    mov     r3, r3, lsr #16
    strh    r3, [r2]
    strh    r4, [r2]
    mov     r4, r4, lsr #16
    strh    r4, [r2]
.w_end4_a:

    tst     r1, #2              /* 2 or more halfwords left? */
    ldrne   r3, [r0], #4
    strhne  r3, [r2]
    movne   r3, r3, lsr #16
    strhne  r3, [r2]

    tst     r1, #1              /* one halfword left? */
    ldrhne  r3, [r0], #2
    strhne  r3, [r2]

    ldmpc   regs=r4-r5

.w_end:
    .size   copy_write_sectors,.w_end-copy_write_sectors