// File: /workspace/github_c_makefile_star2_O2/aap[P]pdp11/rk11.c
// Function: svc_rk05
void
svc_rk05(RK05 *drive, RK11 *rk, int selected)
{
	if(drive->fp == nil){
		reset_rk05(drive);
		return;
	}

	drive->rkds |= DRY | SOK;

	// movement
	// TODO: do some actual timing here?

	// move head (seek)
	drive->rkds &= ~ARDY;
	if(drive->cyl != drive->newcyl){
		if(drive->cyl_timer)
			drive->cyl_timer--;
		if(drive->cyl_timer == 0){
			if(drive->cyl < drive->newcyl)
				drive->cyl++;
			else
				drive->cyl--;
			// some fake value, should be around 640us
			drive->cyl_timer = 500;	
		}

		// search complete
		if(drive->cyl == drive->newcyl &&
		   drive->func == FR_SEEK){
			rk->rkds = (drive-rk->drives)<<13;
			rk->rkcs |= SCP;
			drive->func = 0;
			rk->done = 1;
		}
	}else if(drive->func == 0)
		drive->rkds |= ARDY;

	drive->surf = (rk->rkda>>4)&1;

	// transfer data to/from sector we're interested in
	if(selected && ISXFER(drive->func) &&
	   drive->cyl == drive->newcyl && (rk->rkda&017) == drive->sc){

		// beginning of new sector
		if(drive->wc == 0){
			int pos = blockaddr(drive->cyl, drive->surf, drive->sc)*512;
			fseek(drive->fp, pos, 0);
			fread(rk->buf, 1, 512, drive->fp);
			// rewind so we can write back easily
			fseek(drive->fp, pos, 0);

			rk->state = 1;
		}

		// transfer one word
		if(rk->state == 1 && rk->rkwc){
			rk->rkwc++;

			if(drive->func == FR_READ){
				drive->dsb = WD(rk->buf[drive->wc*2+1], rk->buf[drive->wc*2]);
				rk->rkdb = drive->dsb;

				// write to bus
				rk->bus->data = rk->rkdb;
				rk->bus->addr = busaddr(rk);
				if(dato_bus(rk->bus))
					rkerr(drive, rk, NXM);
			}else if(drive->func == FR_WRITE ||
				 drive->func == FR_WRITE_CHECK){

				// read from bus
				rk->bus->addr = busaddr(rk);
				if(dati_bus(rk->bus))
					rkerr(drive, rk, NXM);
				rk->rkdb = rk->bus->data;

				drive->dsb = rk->rkdb;
				rk->buf[drive->wc*2] = drive->dsb;
				rk->buf[drive->wc*2+1] = drive->dsb>>8;
			}

			// transfer finished
			if(rk->rkwc == 0)
				// have to keep the function around till
				// the end of the block
				rk->done = 1;
		}
	}

	// rotate
	drive->wc++;
	if(drive->wc >= 256){

		// end of sector
		drive->wc = 0;

		// end of current sector
		if(selected && ISXFER(drive->func) &&
		   drive->cyl == drive->newcyl && (rk->rkda&017) == drive->sc){

			if(rk->state == 1){
				count(drive, rk);

				// write back
				if(drive->func == FR_WRITE ||
				   drive->func == FR_WRITE_CHECK)
					fwrite(rk->buf, 1, 512, drive->fp);
				rk->state = 0;
			}

			// end of transfer
			if(rk->rkwc == 0){
				rk->rkcs |= RDY;
				drive->func = 0;
			}
		}

		drive->rkds &= ~SOK;
		drive->sc++;
		// should be one revolution every 40ms
		if(drive->sc >= 12)
			drive->sc = 0;
	}
	drive->rkds &= ~017;
	drive->rkds |= drive->sc;
	if((rk->rkda&017) == drive->sc)
		drive->rkds |= SC_EQ_SA;
	else
		drive->rkds &= ~SC_EQ_SA;
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r12
pushq %rbx
movq %rdi, %rbx
movq (%rdi), %rdi
testq %rdi, %rdi
je 0x5d8e
movl %edx, %ebp
movq %rsi, %r14
movl 0xc(%rbx), %edx
movl 0x1c(%rbx), %eax
movl %edx, %ecx
andl $0xfffffe3f, %ecx       # imm = 0xFFFFFE3F
orl $0x180, %ecx            # imm = 0x180
movw %cx, 0xc(%rbx)
movl 0x20(%rbx), %ecx
cmpl %ecx, %eax
jne 0x5da7
cmpl $0x0, 0x10(%rbx)
jne 0x5e04
orl $0x1c0, %edx            # imm = 0x1C0
movw %dx, 0xc(%rbx)
jmp 0x5e04
movw $0x800, 0xc(%rbx)       # imm = 0x800
andq $0x0, 0x10(%rbx)
andl $0x0, 0x20(%rbx)
andw $0x0, 0x2c(%rbx)
jmp 0x60a3
movl 0x14(%rbx), %edx
testl %edx, %edx
je 0x5db5
decl %edx
movl %edx, 0x14(%rbx)
jne 0x5dcb
xorl %edx, %edx
cmpl %ecx, %eax
setl %dl
leal (%rax,%rdx,2), %eax
decl %eax
movl %eax, 0x1c(%rbx)
movl $0x1f4, 0x14(%rbx)      # imm = 0x1F4
cmpl %ecx, %eax
jne 0x5e04
cmpl $0x4, 0x10(%rbx)
movl %ecx, %eax
jne 0x5e04
movq %rbx, %rax
subq %r14, %rax
addq $-0x28, %rax
pushq $0x30
popq %rsi
cqto
idivq %rsi
shll $0xd, %eax
movw %ax, 0x8(%r14)
orb $0x20, 0xd(%r14)
andl $0x0, 0x10(%rbx)
movl $0x1, 0x1c(%r14)
movl %ecx, %eax
movzwl 0x12(%r14), %edx
movl %edx, %esi
shrl $0x4, %esi
andl $0x1, %esi
movl %esi, 0x24(%rbx)
pushq $0x2e
popq %r12
testl %ebp, %ebp
je 0x5f6f
movl 0x10(%rbx), %r8d
btl %r8d, %r12d
jae 0x5f6f
cmpl %ecx, %eax
jne 0x5f6f
andl $0xf, %edx
cmpl %edx, 0x18(%rbx)
jne 0x5f6f
cmpl $0x0, 0x28(%rbx)
je 0x5e54
cmpl $0x1, 0x18(%r14)
je 0x5e9a
jmp 0x5f6f
leal (%rsi,%rcx,2), %eax
cltq
imulq $0xc, %rax, %r15
addq %rdx, %r15
shlq $0x9, %r15
movq %r15, %rsi
xorl %edx, %edx
callq 0x2170
leaq 0x1a8(%r14), %rdi
movq (%rbx), %rcx
pushq $0x1
popq %rsi
movl $0x200, %edx            # imm = 0x200
callq 0x2050
movq (%rbx), %rdi
movq %r15, %rsi
xorl %edx, %edx
callq 0x2170
movl $0x1, 0x18(%r14)
movzwl 0xe(%r14), %eax
testw %ax, %ax
je 0x5f6f
incl %eax
movw %ax, 0xe(%r14)
movl 0x10(%rbx), %ecx
cmpl $0x1, %ecx
je 0x5f0b
cmpl $0x3, %ecx
je 0x5f0b
cmpl $0x2, %ecx
jne 0x5f62
movslq 0x28(%rbx), %rax
movzwl 0x1a8(%r14,%rax,2), %eax
movw %ax, 0x2c(%rbx)
movw %ax, 0x16(%r14)
movq (%r14), %rcx
movw %ax, 0xc(%rcx)
movq %r14, %rdi
callq 0x6178
movq (%r14), %rdi
movl %eax, 0x8(%rdi)
callq 0x238e
testl %eax, %eax
je 0x5f5d
movq %rbx, %rdi
movq %r14, %rsi
movl $0x400, %edx            # imm = 0x400
callq 0x5c4e
jmp 0x5f5d
movq %r14, %rdi
callq 0x6178
movq (%r14), %rdi
movl %eax, 0x8(%rdi)
callq 0x235f
testl %eax, %eax
je 0x5f32
movq %rbx, %rdi
movq %r14, %rsi
movl $0x400, %edx            # imm = 0x400
callq 0x5c4e
movq (%r14), %rax
movzwl 0xc(%rax), %eax
movw %ax, 0x16(%r14)
movw %ax, 0x2c(%rbx)
movslq 0x28(%rbx), %rcx
movb %al, 0x1a8(%r14,%rcx,2)
movb 0x2d(%rbx), %al
movslq 0x28(%rbx), %rcx
movb %al, 0x1a9(%r14,%rcx,2)
movzwl 0xe(%r14), %eax
testw %ax, %ax
jne 0x5f6f
movl $0x1, 0x1c(%r14)
movl 0x28(%rbx), %eax
leal 0x1(%rax), %ecx
movl %ecx, 0x28(%rbx)
cmpl $0xfe, %eax
jle 0x607d
andl $0x0, 0x28(%rbx)
testl %ebp, %ebp
je 0x6061
movl 0x10(%rbx), %eax
btl %eax, %r12d
jae 0x6061
movl 0x1c(%rbx), %ecx
cmpl 0x20(%rbx), %ecx
jne 0x6061
movzwl 0x12(%r14), %ecx
movl %ecx, %edx
andl $0xf, %edx
cmpl %edx, 0x18(%rbx)
jne 0x6061
cmpl $0x1, 0x18(%r14)
jne 0x6050
cmpw $0x0, 0xe(%r14)
je 0x602c
leal 0x1(%rcx), %edx
movl %edx, %esi
andl $0xf, %esi
movl %edx, %edi
andl $-0x10, %edi
addl $0x10, %edi
cmpw $0xc, %si
cmovnel %edx, %edi
movzwl %di, %ebp
movl $0xe000, %edx           # imm = 0xE000
andl %edx, %edi
andl %edx, %ecx
cmpl %ecx, %edi
je 0x6013
movq %rbx, %rdi
movq %r14, %rsi
movl $0x4000, %edx           # imm = 0x4000
callq 0x5c4e
movzwl 0x12(%r14), %ecx
andl $0xe000, %ecx           # imm = 0xE000
movl 0x10(%rbx), %eax
movl %ebp, %edx
andl $0x1fff, %edx           # imm = 0x1FFF
orl %edx, %ecx
movw %cx, 0x12(%r14)
shrl $0x5, %ebp
movzbl %bpl, %ecx
movl %ecx, 0x20(%rbx)
orl $0x2, %eax
cmpl $0x3, %eax
jne 0x604b
leaq 0x1a8(%r14), %rdi
movq (%rbx), %rcx
pushq $0x1
popq %rsi
movl $0x200, %edx            # imm = 0x200
callq 0x2200
andl $0x0, 0x18(%r14)
cmpw $0x0, 0xe(%r14)
jne 0x6061
orb $-0x80, 0xc(%r14)
andl $0x0, 0x10(%rbx)
movzwl 0xc(%rbx), %eax
andl $0xfeff, %eax           # imm = 0xFEFF
movl 0x18(%rbx), %edx
leal 0x1(%rdx), %esi
xorl %ecx, %ecx
cmpl $0xb, %edx
cmovll %esi, %ecx
movl %ecx, 0x18(%rbx)
jmp 0x6084
movzwl 0xc(%rbx), %eax
movl 0x18(%rbx), %ecx
andl $-0x10, %eax
orl %ecx, %eax
movzwl 0x12(%r14), %edx
andl $0xf, %edx
cmpl %edx, %ecx
jne 0x609a
orl $0x10, %eax
jmp 0x609f
andl $0xffef, %eax           # imm = 0xFFEF
movw %ax, 0xc(%rbx)
popq %rbx
popq %r12
popq %r14
popq %r15
popq %rbp
retq