// File: /workspace/github_c_makefile_star2_O3/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 %r15
pushq %r14
pushq %r12
pushq %rbx
pushq %rax
movq %rdi, %rbx
movq (%rdi), %rdi
testq %rdi, %rdi
je 0x619a
movq %rsi, %r14
movl 0xc(%rbx), %r8d
movl 0x1c(%rbx), %esi
movl %r8d, %eax
andl $0xfffffe3f, %eax       # imm = 0xFFFFFE3F
orl $0x180, %eax            # imm = 0x180
movw %ax, 0xc(%rbx)
movl 0x20(%rbx), %ecx
cmpl %ecx, %esi
jne 0x61ba
cmpl $0x0, 0x10(%rbx)
jne 0x6221
orl $0x1c0, %r8d            # imm = 0x1C0
movw %r8w, 0xc(%rbx)
movl %r8d, %eax
jmp 0x6221
movw $0x800, 0xc(%rbx)       # imm = 0x800
movq $0x0, 0x10(%rbx)
movl $0x0, 0x20(%rbx)
movw $0x0, 0x2c(%rbx)
jmp 0x6481
movl 0x14(%rbx), %r8d
testl %r8d, %r8d
je 0x61cc
decl %r8d
movl %r8d, 0x14(%rbx)
jne 0x61e5
xorl %r8d, %r8d
cmpl %ecx, %esi
setl %r8b
leal (%rsi,%r8,2), %esi
decl %esi
movl %esi, 0x1c(%rbx)
movl $0x1f4, 0x14(%rbx)      # imm = 0x1F4
cmpl %ecx, %esi
jne 0x6221
cmpl $0x4, 0x10(%rbx)
jne 0x621f
movl %ebx, %esi
subl %r14d, %esi
shll $0x9, %esi
addl $0xffffb000, %esi       # imm = 0xFFFFB000
andl $0x1ffe000, %esi        # imm = 0x1FFE000
leal (%rsi,%rsi,2), %esi
movw %si, 0x8(%r14)
orb $0x20, 0xd(%r14)
movl $0x0, 0x10(%rbx)
movl $0x1, 0x1c(%r14)
movl %ecx, %esi
movzwl 0x12(%r14), %r8d
movl %r8d, %r9d
shrl $0x4, %r9d
andl $0x1, %r9d
movl %r9d, 0x24(%rbx)
testl %edx, %edx
je 0x6276
leaq 0x10(%rbx), %r12
movl 0x10(%rbx), %eax
movl $0x2e, %edx
btl %eax, %edx
jae 0x63bc
cmpl %ecx, %esi
jne 0x63bc
andl $0xf, %r8d
cmpl %r8d, 0x18(%rbx)
jne 0x63bc
cmpl $0x0, 0x28(%rbx)
je 0x629c
cmpl $0x1, 0x18(%r14)
je 0x62e6
jmp 0x63bc
movl 0x28(%rbx), %ecx
leal 0x1(%rcx), %edx
movl %edx, 0x28(%rbx)
cmpl $0xfe, %ecx
jle 0x645f
leaq 0x28(%rbx), %r12
movl $0x0, (%r12)
jmp 0x643f
leal (%r9,%rcx,2), %eax
cltq
leaq (%rax,%rax,2), %rax
leaq (%r8,%rax,4), %r15
shlq $0x9, %r15
movq %r15, %rsi
xorl %edx, %edx
callq 0x2170
leaq 0x1a8(%r14), %rdi
movq (%rbx), %rcx
movl $0x1, %esi
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 0x63bc
incl %eax
movw %ax, 0xe(%r14)
movl (%r12), %ecx
cmpl $0x1, %ecx
je 0x6358
cmpl $0x3, %ecx
je 0x6358
cmpl $0x2, %ecx
jne 0x63af
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 0x657f
movq (%r14), %rdi
movl %eax, 0x8(%rdi)
callq 0x23c4
testl %eax, %eax
je 0x63aa
orl $0x800400, 0xa(%r14)    # imm = 0x800400
movl $0x0, (%r12)
jmp 0x63aa
movq %r14, %rdi
callq 0x657f
movq (%r14), %rdi
movl %eax, 0x8(%rdi)
callq 0x2392
testl %eax, %eax
je 0x637f
orl $0x800400, 0xa(%r14)    # imm = 0x800400
movl $0x0, (%r12)
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 0x63bc
movl $0x1, 0x1c(%r14)
movl 0x28(%rbx), %eax
leal 0x1(%rax), %ecx
movl %ecx, 0x28(%rbx)
cmpl $0xfe, %eax
jle 0x645b
movl $0x0, 0x28(%rbx)
movl 0x10(%rbx), %eax
movl $0x2e, %ecx
btl %eax, %ecx
jae 0x643f
movl 0x1c(%rbx), %eax
cmpl 0x20(%rbx), %eax
jne 0x643f
movzwl 0x12(%r14), %eax
andl $0xf, %eax
cmpl %eax, 0x18(%rbx)
jne 0x643f
cmpl $0x1, 0x18(%r14)
jne 0x6437
movq %rbx, %rdi
movq %r14, %rsi
callq 0x65b3
movl 0x10(%rbx), %eax
orl $0x2, %eax
cmpl $0x3, %eax
jne 0x642f
leaq 0x1a8(%r14), %rdi
movq (%rbx), %rcx
movl $0x1, %esi
movl $0x200, %edx            # imm = 0x200
callq 0x2200
movl $0x0, 0x18(%r14)
cmpw $0x0, 0xe(%r14)
je 0x648d
movzwl 0xc(%rbx), %eax
andl $0xfeff, %eax           # imm = 0xFEFF
movl 0x18(%rbx), %ecx
leal 0x1(%rcx), %edx
xorl %esi, %esi
cmpl $0xb, %ecx
cmovll %edx, %esi
movl %esi, 0x18(%rbx)
jmp 0x645f
movzwl 0xc(%rbx), %eax
andl $-0x10, %eax
movl 0x18(%rbx), %ecx
orl %ecx, %eax
movzwl 0x12(%r14), %edx
andl $0xf, %edx
cmpl %edx, %ecx
jne 0x6478
orl $0x10, %eax
jmp 0x647d
andl $0xffef, %eax           # imm = 0xFFEF
movw %ax, 0xc(%rbx)
addq $0x8, %rsp
popq %rbx
popq %r12
popq %r14
popq %r15
retq
orb $-0x80, 0xc(%r14)
jmp 0x628f