PirateSWD.py

import serial
import serial.tools.list_ports

from SWDErrors import *

class PirateSWD:
    BUSPIRATE_VID_PID = "04d8:fb00"

    def __init__ (self, f = None, vreg = False):
        if not f:
            try:
                f, _, _ = serial.tools.list_ports.grep(BUSPIRATE_VID_PID).next()
            except StopIteration:
                raise SWDInitError("Can't find BusPirate")

        self.port = serial.Serial(port = f, baudrate = 115200, timeout = 0.01)
        self.resetBP(vreg = vreg)
        self.sendBytes([0xFF] * 8)
        self.sendBytes([0x79, 0xE7]) # activate SWD interface
        self.resyncSWD()

    def resetBP (self, vreg = False):
        self.expected = 9999
        self.clear()
        self.port.write(bytearray([0x0F]))
        while self.port.read(5) != "BBIO1":
            self.clear(9999)
            self.port.write(bytearray([0x00]))
        self.port.write(bytearray([0x05]))
        if self.port.read(4) != "RAW1":
            raise SWDInitError("error initializing bus pirate")
        if vreg:
            self.port.write(bytearray([0x48]))  # enable voltage regulator output
        self.port.write(bytearray([0x63,0x88])) # set speed to 400 kHz, enable output pins
        self.clear(9999)

    def setPower(self, enable=True):
        self.port.write(bytearray([0x48 if enable else 0x40]))
        self.port.read(1)

    def tristatePins(self, vreg=False):
        self.clear(9999)
        self.port.write(bytearray([0x00])) # exit RAW mode, unfortunately this kills power and generally resets a bunch of stuff
        if self.port.read(5) != "BBIO1":
            raise SWDInitError("unexpected response from bus pirate")
        if vreg:
            self.port.write(bytearray([0xc0])) # re-enable power
        self.port.write(bytearray([0x5f])) # disable all output pins
        self.clear(9999)

    # this is the fastest port-clearing scheme I could devise
    def clear (self, more = 0):
        vals = self.port.read(self.expected + more)
        self.expected = 0
        return vals[-more:]

    def readBits (self, count):
        self.port.write(bytearray([0x07] * count))
        return [ord(x) for x in self.clear(count)]

    def skipBits (self, count):
        self.port.write(bytearray([0x07] * count))
        self.expected = self.expected + count

    def readBytes (self, count):
        self.port.write(bytearray([0x06] * count))
        return [ord(x) for x in self.clear(count)]

    def sendBytes (self, data):
        self.port.write(bytearray([0x10 + ((len(data) - 1) & 0x0F)] + data))
        self.expected = self.expected + 1 + len(data)

    def resyncSWD (self):
        self.sendBytes([0xFF] * 8)
        self.sendBytes([0x00] * 8)

    def readSWD (self, ap, register):
        # transmit the opcode
        self.sendBytes([calcOpcode(ap, register, True)])
        # check the response
        ack = self.readBits(3)
        if ack[0:3] != [1,0,0]:
            if   ack[0:3] == [0,1,0]:
                raise SWDWaitError(ack[0:3])
            elif ack[0:3] == [0,0,1]:
                raise SWDFaultError(ack[0:3])
            else:
                raise SWDProtocolError(ack[0:3])
        # read the next 4 bytes
        data = [reverseBits(b) for b in self.readBytes(4)]
        data.reverse()
        # read the parity bit and turnaround period
        extra = self.readBits(3)
        # check the parity
        if sum([bitCount(x) for x in data[0:4]]) % 2 != extra[0]:
            raise SWDParityError()
        # idle clocking to allow transactions to complete
        self.sendBytes([0x00])
        # return the data
        return (data[0] << 24) | (data[1] << 16) | (data[2] << 8) | data[3]

    def writeSWD (self, ap, register, data, ignoreACK = False):
        # transmit the opcode
        self.sendBytes([calcOpcode(ap, register, False)])
        # check the response if required
        if ignoreACK:
            self.skipBits(5)
        else:
            ack = self.readBits(5)
            if ack[0:3] != [1,0,0]:
                if ack[0:3] == [0,1,0]:
                    raise SWDWaitError(ack[0:3])
                elif ack[0:3] == [0,0,1]:
                    raise SWDFaultError(ack[0:3])
                else:
                    raise SWDProtocolError(ack[0:3])
        # mangle the data endianness
        payload = [0x00, 0x00, 0x00, 0x00, 0x00, 0x00]
        payload[0] = reverseBits((data >>  0) & 0xFF)
        payload[1] = reverseBits((data >>  8) & 0xFF)
        payload[2] = reverseBits((data >> 16) & 0xFF)
        payload[3] = reverseBits((data >> 24) & 0xFF)
        # add the parity bit
        if sum([bitCount(x) for x in payload[0:4]]) % 2:
            payload[4] = 0x80
        # output the data, idle clocking is on the end of the payload
        self.sendBytes(payload)

def bitCount(int_type):
    count = 0
    while(int_type):
        int_type &= int_type - 1
        count += 1
    return(count)

def reverseBits (x):
    a = ((x & 0xAA) >> 1) | ((x & 0x55) << 1)
    b = ((a & 0xCC) >> 2) | ((a & 0x33) << 2)
    c = ((b & 0xF0) >> 4) | ((b & 0x0F) << 4)
    return c

def calcOpcode (ap, register, read):
    opcode = 0x00
    opcode = opcode | (0x20 if read else 0x00)
    opcode = opcode | (0x40 if ap else 0x00)
    opcode = opcode | ((register & 0x01) << 4) | ((register & 0x02) << 2)
    opcode = opcode | ((bitCount(opcode) & 1) << 2)
    opcode = opcode | 0x81
    return opcode