4 files changed, 57 insertions, 2 deletions

diff --git a/blastem.c b/blastem.c
index b5453b4..aa2fc90 100644
--- a/blastem.c
+++ b/blastem.c
@@ -167,8 +167,8 @@ m68k_context * vdp_port_read(uint32_t vdp_port, m68k_context * context)
 		} else if(vdp_port < 8) {
 			context->value = vdp_control_port_read(v_context);
 		} else {
-			//TODO: Implement H/V counter
-			context->value = 0;
+			context->value = vdp_hv_counter_read(v_context);
+			//printf("HV Counter: %X at cycle %d\n", context->value, v_context->cycles);
 		}
 		context->current_cycle = v_context->cycles/MCLKS_PER_68K;
 	} else {
diff --git a/runtime.S b/runtime.S
index 5debd57..a383130 100644
--- a/runtime.S
+++ b/runtime.S
@@ -369,6 +369,8 @@ m68k_read_byte_scratch1:
 	jbe cart_b
 	cmp $0xE00000, %ecx
 	jae workram_b
+	cmp $0xC00000, %ecx
+	jae vdp_psg_b
 	cmp $0xA10000, %ecx
 	jb not_io_b
 	cmp $0xA12000, %ecx
@@ -378,6 +380,17 @@ not_io_b:
 	xor %cl, %cl
 	dec %cl
 	ret
+vdp_psg_b:
+	test $0x2700E0, %ecx
+	jnz crash
+	and $0x1F, %ecx
+	bt $0, %ecx
+	jnc vdp_swap
+	jmp do_vdp_port_read
+vdp_swap:
+	call do_vdp_port_read
+	shr $8, %cx
+	ret
 workram_b:
 	/* deal with byte swapping */
 	xor $1, %ecx
diff --git a/vdp.c b/vdp.c
index 27b2987..241cfb7 100644
--- a/vdp.c
+++ b/vdp.c
@@ -1092,6 +1092,10 @@ uint16_t vdp_control_port_read(vdp_context * context)
 	if (context->flags & FLAG_DMA_RUN) {
 		value |= 0x20;
 	}
+	uint32_t line= context->cycles / MCLKS_LINE;
+	if (line >= (context->latched_mode & BIT_PAL ? PAL_ACTIVE : NTSC_ACTIVE)) {
+		value |= 0x8;
+	}
 	//TODO: Lots of other bits in status port
 	return value;
 }
@@ -1131,6 +1135,43 @@ uint16_t vdp_data_port_read(vdp_context * context)
 	return value;
 }
 
+uint16_t vdp_hv_counter_read(vdp_context * context)
+{
+	uint32_t line= context->cycles / MCLKS_LINE;
+	if (!line) {
+		line = 0xFF;
+	} else {
+		line--;
+		if (line > 0xEA) {
+			line = (line + 0xFA) & 0xFF;
+		}
+	}
+	uint32_t linecyc = context->cycles % MCLKS_LINE;
+	if (context->latched_mode & BIT_H40) {
+		linecyc /= 8;
+		if (linecyc >= 86) {
+			linecyc -= 86;
+		} else {
+			linecyc += 334;
+		}
+		if (linecyc > 0x16C) {
+			linecyc += 92;
+		}
+	} else {
+		linecyc /= 10;
+		if (linecyc >= 74) {
+			linecyc -= 74;
+		} else {
+			linecyc += 268;
+		}
+		if (linecyc > 0x127) {
+			linecyc += 170;
+		}
+	}
+	linecyc &= 0xFF;
+	return (line << 8) | linecyc;
+}
+
 void vdp_adjust_cycles(vdp_context * context, uint32_t deduction)
 {
 	context->cycles -= deduction;
diff --git a/vdp.h b/vdp.h
index baafccf..8a91b79 100644
--- a/vdp.h
+++ b/vdp.h
@@ -127,6 +127,7 @@ int vdp_control_port_write(vdp_context * context, uint16_t value);
 void vdp_data_port_write(vdp_context * context, uint16_t value);
 uint16_t vdp_control_port_read(vdp_context * context);
 uint16_t vdp_data_port_read(vdp_context * context);
+uint16_t vdp_hv_counter_read(vdp_context * context);
 void vdp_adjust_cycles(vdp_context * context, uint32_t deduction);
 
 #endif //VDP_H_