Breakpoint Hooking Problem

 
bool bpSetHooks(void)
{
	if (!pExceptionHandler || !bpBackupRegisters())
		return false;
 
	CONTEXT ctx = { CONTEXT_DEBUG_REGISTERS };
 
	// Register #1
	ctx.Dr0 = gBreakPoints.bp[0].dwAddr;
	ctx.Dr7 = 0x00000001;
 
	// Register #2
	ctx.Dr1 = gBreakPoints.bp[1].dwAddr;
	ctx.Dr7 |= 0x00000004;
 
	// Register #3
	ctx.Dr2 = gBreakPoints.bp[2].dwAddr;
	ctx.Dr7 |= 0x00000010;
 
	// Register #4
	ctx.Dr3 = gBreakPoints.bp[3].dwAddr;
	ctx.Dr7 |= 0x00000040;	
	return (SetThreadContext(gBreakPoints.hThread, &ctx) == TRUE);
 
	return true;
}
 

// Register #3
	ctx.Dr2 = 0x4003000; //gBreakPoints.bp[2].dwAddr;
	ctx.Dr7 |= 0x00000010;
 
	// Register #4
	ctx.Dr3 = 0x4004000; //gBreakPoints.bp[3].dwAddr;
	ctx.Dr7 |= 0x00000040;	// todo: dynamic loop here

 
bool bpInitialize(breakpoints_t *pBreakPoints)
{
	memcpy(&gBreakPoints, pBreakPoints, sizeof(gBreakPoints));
 
	if (gBreakPoints.hThread == NULL)
		gBreakPoints.hThread = GetCurrentThread();
 
	for (int i = 0; i < 4; i++)
	{
		if (*(gBreakPoints.bp[i].pSystemFunction) != NULL)
			continue;
 
		gBreakPoints.bp[i].dwAddr = gBreakPoints.bp[i].dwFunctionAddr;
 
		// length of opcode + 0x5
		size_t sStubSize = oplen((BYTE *)gBreakPoints.bp[i].dwAddr);
		*(gBreakPoints.bp[i].pSystemFunction) = malloc((sStubSize + 0x5));
 
 
		// copy the opcodes to the buffer
		memcpy(*(gBreakPoints.bp[i].pSystemFunction), (PVOID)gBreakPoints.bp[i].dwAddr, sStubSize);
 
		// add a jmp instruction to the original system function at the end of the buffer
		*(PBYTE)((DWORD)*(gBreakPoints.bp[i].pSystemFunction) + (DWORD)sStubSize) = 0xE9;
		*(PDWORD)((DWORD)*(gBreakPoints.bp[i].pSystemFunction) + (DWORD)sStubSize + 0x1) = (((DWORD)gBreakPoints.bp[i].dwAddr + (DWORD)sStubSize) - ((DWORD)*(gBreakPoints.bp[i].pSystemFunction) + (DWORD)sStubSize + 0x5));
 
	}
 
	if (pExceptionHandler == NULL)
		pExceptionHandler = AddVectoredExceptionHandler(1, ExceptionHandler);
	return true;
}