diff --git a/armsrc/appmain.c b/armsrc/appmain.c index 03ce3750b..534cef648 100644 --- a/armsrc/appmain.c +++ b/armsrc/appmain.c @@ -1444,9 +1444,9 @@ void UsbPacketReceived(uint8_t *packet, int len) { break; case CMD_PING: #ifdef WITH_FPC_HOST - cmd_send(CMD_ACK, reply_via_fpc, 0, 0, 0, 0); + cmd_send(CMD_ACK, reply_via_fpc, c->d.asDwords[0], c->d.asDwords[(c->arg[0]-1)/4], 0, 0); #else - cmd_send(CMD_ACK, 0, 0, 0, 0, 0); + cmd_send(CMD_ACK, 0, c->d.asDwords[0], c->d.asDwords[(c->arg[0]-1)/4], 0, 0); #endif break; #ifdef WITH_LCD @@ -1553,24 +1553,73 @@ void __attribute__((noreturn)) AppMain(void) { usb_disable(); usb_enable(); - uint8_t rx[sizeof(UsbCommand)]; + // Worst case: Command as large as the old one but encapsulated in NG style + uint8_t rx[sizeof(UsbCommandNGPreamble) + sizeof(UsbCommand) + sizeof(UsbCommandNGPostamble)]; + UsbCommandNGPreamble *pre = (UsbCommandNGPreamble *)rx; + UsbCommandNGPostamble *post = (UsbCommandNGPostamble *)(rx + sizeof(UsbCommandNGPreamble) + sizeof(UsbCommand)); for (;;) { WDT_HIT(); // Check if there is a usb packet available if (usb_poll_validate_length()) { - if (usb_read(rx, sizeof(rx))) { -#ifdef WITH_FPC_HOST - reply_via_fpc = 0; -#endif - UsbPacketReceived(rx, sizeof(rx)); + bool error = false; + size_t bytes = usb_read_ng(rx, sizeof(UsbCommandNGPreamble) + sizeof(UsbCommandNG)); + if (bytes == sizeof(UsbCommandNGPreamble) + sizeof(UsbCommandNG)) { + if (pre->magic == USB_PREAMBLE_MAGIC) { // New style NG command +// Dbprintf("Packet frame NG incoming!"); + use_cmd_ng = true; + if (pre->length > USB_CMD_DATA_SIZE) { + Dbprintf("Packet frame with incompatible length: 0x%04x", pre->length); + error = true; + } + if ((!error) && (pre->length > 0)) { // Get the variable length payload + bytes = usb_read_ng(rx + sizeof(UsbCommandNGPreamble) + sizeof(UsbCommandNG), pre->length); + if (bytes != pre->length) { + Dbprintf("Packet frame error variable part too short? %d/%d", bytes, pre->length); + error = true; + } + } + if (!error) { // Get the postamble + bytes = usb_read_ng(rx + sizeof(UsbCommandNGPreamble) + sizeof(UsbCommand), sizeof(UsbCommandNGPostamble)); + if ((bytes != sizeof(UsbCommandNGPostamble)) || (post->magic != USB_POSTAMBLE_MAGIC)) { + Dbprintf("Packet frame error no postamble magic found"); + error = true; + } + // TODO check also CRC... + } + if (!error) { +// Dbprintf("Packet frame NG fully received"); + #ifdef WITH_FPC_HOST + reply_via_fpc = false; + #endif + UsbPacketReceived(rx + sizeof(UsbCommandNGPreamble), sizeof(UsbCommand)); + } + + } else { // Old style command + bytes = usb_read_ng(rx + sizeof(UsbCommandNGPreamble) + sizeof(UsbCommandNG), sizeof(UsbCommand) - sizeof(UsbCommandNGPreamble) - sizeof(UsbCommandNG)); + + if (bytes != sizeof(UsbCommand) - sizeof(UsbCommandNGPreamble) - sizeof(UsbCommandNG)) { + Dbprintf("Packet frame error var part too short? %d/%d", bytes, sizeof(UsbCommand) - sizeof(UsbCommandNGPreamble) - sizeof(UsbCommandNG)); + error = true; + } + if (!error) { + use_cmd_ng = false; + #ifdef WITH_FPC_HOST + reply_via_fpc = false; + #endif + UsbPacketReceived(rx, sizeof(UsbCommand)); + } + } + } else { + error = true; } + // TODO if error, shall we resync ? } #ifdef WITH_FPC_HOST // Check if there is a FPC packet available if (usart_readbuffer(rx)) { - reply_via_fpc = 1; + reply_via_fpc = true; UsbPacketReceived(rx, sizeof(rx)); } usart_readcheck(rx, sizeof(rx)); diff --git a/client/cmdhw.c b/client/cmdhw.c index d1ba4274e..655442738 100644 --- a/client/cmdhw.c +++ b/client/cmdhw.c @@ -438,6 +438,31 @@ static int CmdPing(const char *Cmd) { return 0; } +static int CmdPingNG(const char *Cmd) { + uint32_t len = strtol(Cmd, NULL, 0); + if (len > USB_CMD_DATA_SIZE) + len = USB_CMD_DATA_SIZE; + PrintAndLogEx(NORMAL, "Pinging with payload len=%d", len); + clearCommandBuffer(); + UsbCommand resp; + UsbCommand c = {CMD_PING, {len, 0, 0}, {{0}}}; + if (len >= 4) + c.d.asDwords[0] = 0xAABBCCDD; + if (len >= 8) + c.d.asDwords[(len-1)/4] = 0xDDCCBBAA; + SendCommandNG(&c, len); + if (WaitForResponseTimeout(CMD_ACK, &resp, 1000)) { + PrintAndLogEx(NORMAL, "PingNG successful"); + if (len >= 4) + PrintAndLogEx(NORMAL, "%08x -> %08x", 0xAABBCCDD, resp.arg[1]); + if (len >= 8) + PrintAndLogEx(NORMAL, "%08x -> %08x", 0xDDCCBBAA, resp.arg[2]); + } + else + PrintAndLogEx(NORMAL, "PingNG failed"); + return 0; +} + static command_t CommandTable[] = { {"help", CmdHelp, 1, "This help"}, {"detectreader", CmdDetectReader, 0, "['l'|'h'] -- Detect external reader field (option 'l' or 'h' to limit to LF or HF)"}, @@ -454,6 +479,7 @@ static command_t CommandTable[] = { {"version", CmdVersion, 0, "Show version information about the connected Proxmark"}, {"status", CmdStatus, 0, "Show runtime status information about the connected Proxmark"}, {"ping", CmdPing, 0, "Test if the pm3 is responsive"}, + {"pingng", CmdPingNG, 0, "Test if the pm3 is responsive"}, {NULL, NULL, 0, NULL} }; diff --git a/client/comms.c b/client/comms.c index a96823d81..d192b4c90 100644 --- a/client/comms.c +++ b/client/comms.c @@ -25,10 +25,12 @@ static pthread_t USB_communication_thread; // Transmit buffer. static UsbCommand txBuffer; +static uint8_t txBufferNG[sizeof(UsbCommandNGPreamble) + sizeof(UsbCommand) + sizeof(UsbCommandNGPostamble)]; +size_t txBufferNGLen; static bool txBuffer_pending = false; static pthread_mutex_t txBufferMutex = PTHREAD_MUTEX_INITIALIZER; static pthread_cond_t txBufferSig = PTHREAD_COND_INITIALIZER; - + // Used by UsbReceiveCommand as a ring buffer for messages that are yet to be // processed by a command handler (WaitForResponse{,Timeout}) static UsbCommand rxBuffer[CMD_BUFFER_SIZE]; @@ -84,6 +86,50 @@ void SendCommand(UsbCommand *c) { //__atomic_test_and_set(&txcmd_pending, __ATOMIC_SEQ_CST); } +void SendCommandNG(UsbCommand *c, size_t len) { + +#ifdef COMMS_DEBUG + PrintAndLogEx(NORMAL, "Sending %d bytes of payload | cmd %04x\n", len, c->cmd); +#endif + + if (offline) { + PrintAndLogEx(NORMAL, "Sending bytes to proxmark failed - offline"); + return; + } + if (len > USB_CMD_DATA_SIZE) { + PrintAndLogEx(WARNING, "Sending %d bytes of payload is too much, abort", len); + return; + } + + UsbCommandNGPreamble *tx_pre = (UsbCommandNGPreamble *)txBufferNG; + UsbCommandNGPostamble *tx_post = (UsbCommandNGPostamble *)(txBufferNG + sizeof(UsbCommandNGPreamble) + sizeof(UsbCommandNG) + len); + + pthread_mutex_lock(&txBufferMutex); + /** + This causes hangups at times, when the pm3 unit is unresponsive or disconnected. The main console thread is alive, + but comm thread just spins here. Not good.../holiman + **/ + while (txBuffer_pending) { + // wait for communication thread to complete sending a previous commmand + pthread_cond_wait(&txBufferSig, &txBufferMutex); + } + + tx_pre->magic = USB_PREAMBLE_MAGIC; + tx_pre->length = len; + memcpy(txBufferNG + sizeof(UsbCommandNGPreamble), c, sizeof(UsbCommandNG) + len); + // TODO CRC + tx_post->magic = USB_POSTAMBLE_MAGIC; + txBufferNGLen = sizeof(UsbCommandNGPreamble) + sizeof(UsbCommandNG) + len + sizeof(UsbCommandNGPostamble); + txBuffer_pending = true; + + // tell communication thread that a new command can be send + pthread_cond_signal(&txBufferSig); + + pthread_mutex_unlock(&txBufferMutex); + +//__atomic_test_and_set(&txcmd_pending, __ATOMIC_SEQ_CST); +} + /** * @brief This method should be called when sending a new command to the pm3. In case any old * responses from previous commands are stored in the buffer, a call to this method should clear them. @@ -273,9 +319,17 @@ __attribute__((force_align_arg_pointer)) } if (txBuffer_pending) { - if (!uart_send(sp, (uint8_t *) &txBuffer, sizeof(UsbCommand))) { - //counter_to_offline++; - PrintAndLogEx(WARNING, "sending bytes to proxmark failed"); + if (txBufferNGLen) { // NG packet + if (!uart_send(sp, (uint8_t *) &txBufferNG, txBufferNGLen)) { + //counter_to_offline++; + PrintAndLogEx(WARNING, "sending bytes to proxmark failed"); + } + txBufferNGLen = 0; + } else { + if (!uart_send(sp, (uint8_t *) &txBuffer, sizeof(UsbCommand))) { + //counter_to_offline++; + PrintAndLogEx(WARNING, "sending bytes to proxmark failed"); + } } txBuffer_pending = false; diff --git a/client/comms.h b/client/comms.h index 68f790eb1..d6704617b 100644 --- a/client/comms.h +++ b/client/comms.h @@ -52,6 +52,7 @@ bool IsOffline(void); void *uart_receiver(void *targ); void SendCommand(UsbCommand *c); +void SendCommandNG(UsbCommand *c, size_t len); void clearCommandBuffer(void); #define FLASHMODE_SPEED 460800 diff --git a/common/cmd.c b/common/cmd.c index ab8eeedaa..74f8c00f5 100644 --- a/common/cmd.c +++ b/common/cmd.c @@ -31,15 +31,16 @@ */ #include "cmd.h" +bool use_cmd_ng = false; #ifdef WITH_FPC_HOST // "Session" flag, to tell via which interface next msgs should be sent: USB or FPC USART -bool reply_via_fpc = 0; +bool reply_via_fpc = false; extern void Dbprintf(const char *fmt, ...); #define Dbprintf_usb(...) {\ - reply_via_fpc = 0;\ + reply_via_fpc = false;\ Dbprintf(__VA_ARGS__);\ - reply_via_fpc = 1;} + reply_via_fpc = true;} #endif uint8_t cmd_send(uint64_t cmd, uint64_t arg0, uint64_t arg1, uint64_t arg2, void *data, size_t len) { diff --git a/common/usb_cdc.c b/common/usb_cdc.c index 010d9d2b7..c66593a33 100644 --- a/common/usb_cdc.c +++ b/common/usb_cdc.c @@ -646,6 +646,65 @@ uint32_t usb_read(uint8_t *data, size_t len) { return nbBytesRcv; } +static uint8_t usb_read_ng_buffer[64]; +static size_t usb_read_ng_bufoff=0; +static size_t usb_read_ng_buflen=0; + +uint32_t usb_read_ng(uint8_t *data, size_t len) { + + if (len == 0) return 0; + + uint8_t bank = btReceiveBank; + uint32_t packetSize, nbBytesRcv = 0; + uint32_t time_out = 0; + + // take first from local buffer + if ( len <= usb_read_ng_buflen ) { + for (uint32_t i = 0; i < len; i++) + data[nbBytesRcv++] = usb_read_ng_buffer[usb_read_ng_bufoff + i]; + usb_read_ng_buflen -= len; + if (usb_read_ng_buflen == 0) + usb_read_ng_bufoff = 0; + else + usb_read_ng_bufoff += len; + return nbBytesRcv; + } else { + for (uint32_t i = 0; i < usb_read_ng_buflen; i++) + data[nbBytesRcv++] = usb_read_ng_buffer[usb_read_ng_bufoff + i]; + len -= usb_read_ng_buflen; + usb_read_ng_buflen = 0; + usb_read_ng_bufoff = 0; + } + + while (len) { + if (!usb_check()) break; + + if ((pUdp->UDP_CSR[AT91C_EP_OUT] & bank)) { + + uint32_t available = (pUdp->UDP_CSR[AT91C_EP_OUT] & AT91C_UDP_RXBYTECNT) >> 16; + packetSize = MIN(available, len); + available -= packetSize; + len -= packetSize; + while (packetSize--) + data[nbBytesRcv++] = pUdp->UDP_FDR[AT91C_EP_OUT]; + // fill the local buffer with the remaining bytes + for (uint32_t i = 0; i < available; i++) + usb_read_ng_buffer[i] = pUdp->UDP_FDR[AT91C_EP_OUT]; + usb_read_ng_buflen = available; + // flip bank + UDP_CLEAR_EP_FLAGS(AT91C_EP_OUT, bank) + if (bank == AT91C_UDP_RX_DATA_BK0) + bank = AT91C_UDP_RX_DATA_BK1; + else + bank = AT91C_UDP_RX_DATA_BK0; + } + if (time_out++ == 0x1fff) break; + } + + btReceiveBank = bank; + return nbBytesRcv; +} + //*---------------------------------------------------------------------------- //* \fn usb_write //* \brief Send through endpoint 2 (device to host) diff --git a/common/usb_cdc.h b/common/usb_cdc.h index a74e3d8c1..d5a4451ea 100644 --- a/common/usb_cdc.h +++ b/common/usb_cdc.h @@ -48,6 +48,8 @@ bool usb_poll(void); bool usb_poll_validate_length(void); uint32_t usb_read(uint8_t *data, size_t len); uint32_t usb_write(const uint8_t *data, const size_t len); +uint32_t usb_read_ng(uint8_t *data, size_t len); +uint32_t usb_write_ng(const uint8_t *data, const size_t len); void SetUSBreconnect(int value); int GetUSBreconnect(void); diff --git a/include/usb_cmd.h b/include/usb_cmd.h index 57f2c46fd..cbef5aa02 100644 --- a/include/usb_cmd.h +++ b/include/usb_cmd.h @@ -35,6 +35,27 @@ typedef struct { } d; } PACKED UsbCommand; +typedef struct { + uint32_t magic; + uint32_t length; // length of the variable part, 0 if none. +} PACKED UsbCommandNGPreamble; + +#define USB_PREAMBLE_MAGIC 0xAA5500FF + +typedef struct { + uint64_t cmd; + uint64_t arg[3]; +} PACKED UsbCommandNG; + +typedef struct { + uint32_t crc; + uint32_t magic; +} PACKED UsbCommandNGPostamble; + +#define USB_POSTAMBLE_MAGIC 0xFF0055AA + +extern bool use_cmd_ng; + #ifdef WITH_FPC_HOST // "Session" flag, to tell via which interface next msgs should be sent: USB or FPC USART extern bool reply_via_fpc;