stk500v2 programmer sketch

I started work on a sketch (may be made into a library later) that implements the stk500v2 protocol for programming atmel AVR microcontrollers. The best progress I have made so far is reading fuses. I'm working on the read flash routine, but it is giving me problems.

Can you be more specific about what kind of problems? What is working, what not? what do you expect and what do you get?

Sure, I am using software SPI. When I send the commands for read flash, the first byte appears to be returned successfully, but all the other bytes return the low byte of the address instead of what was actually read.

I am implementing a routine that sends the command byte, the two bytes for address (load address is implemented), and a 0 byte to retrieve the value. The second command does a bitwise of the command byte and 8 before sending. Every command after the first read command just returns the low byte of the address.

EDIT: Hmm, I think I fixed it. I forgot to save the command byte to a temporary variable before overwriting it with the response data.

Anybody interested in testing this out as I add features?