Fix readMem and writeMem by writing to DP select

The functions readMem and writeMem expect the DP select register to be
set to the same bank as the CSW register when the CSW register is
written to.  If the CSW already has the correct value, writeAP will
return without updating the select register.  This could cause the
readMem and writeMem functions to access the wrong bank resulting
in undefined behavior.

This problem can be reproduced by reading from another bank immediately
before readMem or writeMem.  An example of this is reading the IDR
register right after halting the target processor in the memory test in
basic_test.py:
print "MEM IDR: 0x{0:x}".format(transport.readAP(0xFC))

On the KL25 freedom board the following output is produced:
------ TEST READ / WRITE MEMORY ------
MEM IDR: 0x4770031
READ32/WRITE32
write32 0x81592C8C at 0x20000001
read32 at 0x20000001: 0x4770031
ERROR in READ/WRITE 32

This patch fixes the problem by always updating the DP select in register
in writeAP if it is out of date.  The select register will not be written
unnecessarily since writeDP checks if the select register is up to date
and returns early if it is up to date.