// File: /workspace/github_c_makefile_star2_O1/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 %rbx
pushq %rax
movq %rdi, %rbx
movq (%rdi), %rdi
testq %rdi, %rdi
je 0x6162
movl %edx, %ebp
movq %rsi, %r14
movl 0xc(%rbx), %edx
movl 0x1c(%rbx), %ecx
movl %edx, %eax
andl $0xfffffe3f, %eax       # imm = 0xFFFFFE3F
orl $0x180, %eax            # imm = 0x180
movw %ax, 0xc(%rbx)
movl 0x20(%rbx), %eax
cmpl %eax, %ecx
jne 0x6182
cmpl $0x0, 0x10(%rbx)
jne 0x61e5
orl $0x1c0, %edx            # imm = 0x1C0
movw %dx, 0xc(%rbx)
jmp 0x61e5
movw $0x800, 0xc(%rbx)       # imm = 0x800
movq $0x0, 0x10(%rbx)
movl $0x0, 0x20(%rbx)
movw $0x0, 0x2c(%rbx)
jmp 0x6427
movl 0x14(%rbx), %edx
testl %edx, %edx
je 0x618e
decl %edx
movl %edx, 0x14(%rbx)
cmpl $0x0, 0x14(%rbx)
jne 0x61aa
xorl %edx, %edx
cmpl %eax, %ecx
setl %dl
leal (%rcx,%rdx,2), %ecx
decl %ecx
movl %ecx, 0x1c(%rbx)
movl $0x1f4, 0x14(%rbx)      # imm = 0x1F4
cmpl %eax, 0x1c(%rbx)
jne 0x61e5
cmpl $0x4, 0x10(%rbx)
jne 0x61e5
movl %ebx, %ecx
subl %r14d, %ecx
shll $0x9, %ecx
addl $0xffffb000, %ecx       # imm = 0xFFFFB000
andl $0x1ffe000, %ecx        # imm = 0x1FFE000
leal (%rcx,%rcx,2), %ecx
movw %cx, 0x8(%r14)
orb $0x20, 0xd(%r14)
movl $0x0, 0x10(%rbx)
movl $0x1, 0x1c(%r14)
movzwl 0x12(%r14), %edx
movl %edx, %ecx
shrl $0x4, %ecx
andl $0x1, %ecx
movl %ecx, 0x24(%rbx)
testl %ebp, %ebp
je 0x6356
movl 0x10(%rbx), %esi
movl $0x2e, %r8d
btl %esi, %r8d
jae 0x6356
movl 0x1c(%rbx), %esi
cmpl %eax, %esi
jne 0x6356
andl $0xf, %edx
movl 0x18(%rbx), %eax
cmpl %edx, %eax
jne 0x6356
cmpl $0x0, 0x28(%rbx)
jne 0x6276
leal (%rcx,%rsi,2), %ecx
leal (%rcx,%rcx,2), %ecx
leal (%rax,%rcx,4), %eax
shll $0x9, %eax
movslq %eax, %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)
cmpl $0x1, 0x18(%r14)
jne 0x6356
movzwl 0xe(%r14), %eax
testw %ax, %ax
je 0x6356
incl %eax
movw %ax, 0xe(%r14)
movl 0x10(%rbx), %eax
cmpl $0x1, %eax
je 0x62f3
cmpl $0x3, %eax
je 0x62f3
cmpl $0x2, %eax
jne 0x6346
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 0x651b
movq (%r14), %rdi
movl %eax, 0x8(%rdi)
callq 0x24c4
testl %eax, %eax
je 0x6346
orb $0x4, 0xb(%r14)
orb $-0x80, 0xc(%r14)
movl $0x0, 0x10(%rbx)
jmp 0x6346
movq %r14, %rdi
callq 0x651b
movq (%r14), %rdi
movl %eax, 0x8(%rdi)
callq 0x2492
testl %eax, %eax
je 0x631b
orb $0x4, 0xb(%r14)
orb $-0x80, 0xc(%r14)
movl $0x0, 0x10(%rbx)
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)
cmpw $0x0, 0xe(%r14)
jne 0x6356
movl $0x1, 0x1c(%r14)
movl 0x28(%rbx), %eax
leal 0x1(%rax), %ecx
movl %ecx, 0x28(%rbx)
cmpl $0xff, %eax
jl 0x63fe
movl $0x0, 0x28(%rbx)
testl %ebp, %ebp
je 0x63e9
movl 0x10(%rbx), %eax
movl $0x2e, %ecx
btl %eax, %ecx
jae 0x63e9
movl 0x1c(%rbx), %eax
cmpl 0x20(%rbx), %eax
jne 0x63e9
movzwl 0x12(%r14), %eax
andl $0xf, %eax
cmpl %eax, 0x18(%rbx)
jne 0x63e9
cmpl $0x1, 0x18(%r14)
jne 0x63d5
movq %rbx, %rdi
movq %r14, %rsi
callq 0x654f
movl 0x10(%rbx), %eax
orl $0x2, %eax
cmpl $0x3, %eax
jne 0x63cd
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)
jne 0x63e9
orb $-0x80, 0xc(%r14)
movl $0x0, 0x10(%rbx)
andb $-0x2, 0xd(%rbx)
movl 0x18(%rbx), %eax
leal 0x1(%rax), %ecx
xorl %edx, %edx
cmpl $0xb, %eax
cmovll %ecx, %edx
movl %edx, 0x18(%rbx)
movl 0xc(%rbx), %eax
movl 0x18(%rbx), %ecx
andl $-0x10, %eax
orl %ecx, %eax
movw %ax, 0xc(%rbx)
movzwl 0x12(%r14), %edx
andl $0xf, %edx
cmpl %edx, %ecx
jne 0x641e
orl $0x10, %eax
jmp 0x6423
andl $0xffef, %eax           # imm = 0xFFEF
movw %ax, 0xc(%rbx)
addq $0x8, %rsp
popq %rbx
popq %r14
popq %r15
popq %rbp
retq