diff --git a/armsrc/appmain.c b/armsrc/appmain.c index 534cef648..fd3760bed 100644 --- a/armsrc/appmain.c +++ b/armsrc/appmain.c @@ -632,11 +632,23 @@ void ListenReaderField(int limit) { } } -void UsbPacketReceived(uint8_t *packet, int len) { - UsbCommand *c = (UsbCommand *)packet; - //Dbprintf("received %d bytes, with command: 0x%04x and args: %d %d %d", len, c->cmd, c->arg[0], c->arg[1], c->arg[2]); +static void UsbPacketReceived(bool cmd_ng, uint8_t *packet) { + uint64_t cmd; // To accomodate old cmd, can be reduced to uint16_t once all old cmds are gone. + UsbCommandNGPreamble *pre_ng = (UsbCommandNGPreamble *)packet; + uint8_t *data_ng = packet + sizeof(UsbCommandNGPreamble); + uint16_t datalen_ng = pre_ng->length; - switch (c->cmd) { + // For cmd handlers still using old cmd format: + UsbCommand *c = (UsbCommand *)packet; + if (cmd_ng) { + cmd = pre_ng->cmd; +// Dbprintf("received %d bytes payload, with command: 0x%04x", datalen_ng, cmd); + } else { +// Dbprintf("received %d bytes, with command: 0x%04x and args: %d %d %d", USB_CMD_DATA_SIZE, c->cmd, c->arg[0], c->arg[1], c->arg[2]); + cmd = c->cmd; + } + + switch (cmd) { #ifdef WITH_LF case CMD_SET_LF_T55XX_CONFIG: setT55xxConfig(c->arg[0], (t55xx_config *) c->d.asBytes); @@ -1443,11 +1455,19 @@ void UsbPacketReceived(uint8_t *packet, int len) { SendStatus(); break; case CMD_PING: + if (cmd_ng) { #ifdef WITH_FPC_HOST - cmd_send(CMD_ACK, reply_via_fpc, c->d.asDwords[0], c->d.asDwords[(c->arg[0]-1)/4], 0, 0); + cmd_send(CMD_ACK, reply_via_fpc, ((uint32_t *)data_ng)[0], ((uint32_t *)data_ng)[(datalen_ng-1)/4], 0, 0); #else - cmd_send(CMD_ACK, 0, c->d.asDwords[0], c->d.asDwords[(c->arg[0]-1)/4], 0, 0); + cmd_send(CMD_ACK, 0, ((uint32_t *)data_ng)[0], ((uint32_t *)data_ng)[(c->arg[0]-1)/4], 0, 0); #endif + } else { +#ifdef WITH_FPC_HOST + cmd_send(CMD_ACK, reply_via_fpc, 0, 0, 0, 0); +#else + cmd_send(CMD_ACK, 0, 0, 0, 0, 0); +#endif + } break; #ifdef WITH_LCD case CMD_LCD_RESET: @@ -1488,7 +1508,7 @@ void UsbPacketReceived(uint8_t *packet, int len) { break; } default: - Dbprintf("%s: 0x%04x", "unknown command:", c->cmd); + Dbprintf("%s: 0x%04x", "unknown command:", cmd); break; } } @@ -1553,10 +1573,9 @@ void __attribute__((noreturn)) AppMain(void) { usb_disable(); usb_enable(); - // Worst case: Command as large as the old one but encapsulated in NG style - uint8_t rx[sizeof(UsbCommandNGPreamble) + sizeof(UsbCommand) + sizeof(UsbCommandNGPostamble)]; + uint8_t rx[USB_PACKET_NG_MAXLEN]; UsbCommandNGPreamble *pre = (UsbCommandNGPreamble *)rx; - UsbCommandNGPostamble *post = (UsbCommandNGPostamble *)(rx + sizeof(UsbCommandNGPreamble) + sizeof(UsbCommand)); + UsbCommandNGPostamble *post = (UsbCommandNGPostamble *)(rx + sizeof(UsbCommandNGPreamble) + USB_CMD_DATA_SIZE); for (;;) { WDT_HIT(); @@ -1564,54 +1583,54 @@ void __attribute__((noreturn)) AppMain(void) { // Check if there is a usb packet available if (usb_poll_validate_length()) { bool error = false; - size_t bytes = usb_read_ng(rx, sizeof(UsbCommandNGPreamble) + sizeof(UsbCommandNG)); - if (bytes == sizeof(UsbCommandNGPreamble) + sizeof(UsbCommandNG)) { + size_t bytes = usb_read_ng(rx, sizeof(UsbCommandNGPreamble)); + if (bytes == sizeof(UsbCommandNGPreamble)) { 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); + bytes = usb_read_ng(rx + sizeof(UsbCommandNGPreamble), 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"); + bytes = usb_read_ng(rx + sizeof(UsbCommandNGPreamble) + USB_CMD_DATA_SIZE, sizeof(UsbCommandNGPostamble)); + if (bytes != sizeof(UsbCommandNGPostamble)) { + Dbprintf("Packet frame error fetching postamble"); + error = true; + } + uint8_t first, second; + compute_crc(CRC_14443_A, rx, sizeof(UsbCommandNGPreamble) + pre->length, &first, &second); + if ((first << 8) + second != post->crc) { + Dbprintf("Packet frame CRC error %02X%02X <> %04X", first, second, post->crc); 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)); + UsbPacketReceived(true, rx); } - } 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)); + bytes = usb_read_ng(rx + sizeof(UsbCommandNGPreamble), sizeof(UsbCommand) - sizeof(UsbCommandNGPreamble)); + if (bytes != sizeof(UsbCommand) - sizeof(UsbCommandNGPreamble)) { + Dbprintf("Packet frame error var part too short? %d/%d", bytes, sizeof(UsbCommand) - sizeof(UsbCommandNGPreamble)); error = true; } if (!error) { - use_cmd_ng = false; #ifdef WITH_FPC_HOST reply_via_fpc = false; #endif - UsbPacketReceived(rx, sizeof(UsbCommand)); + UsbPacketReceived(false, rx); } } } else { + Dbprintf("Packet frame preamble too short: %d/%d", bytes, sizeof(UsbCommandNGPreamble)); error = true; } // TODO if error, shall we resync ? @@ -1620,7 +1639,7 @@ void __attribute__((noreturn)) AppMain(void) { // Check if there is a FPC packet available if (usart_readbuffer(rx)) { reply_via_fpc = true; - UsbPacketReceived(rx, sizeof(rx)); + UsbPacketReceived(false, rx); } usart_readcheck(rx, sizeof(rx)); #endif diff --git a/client/Makefile b/client/Makefile index 2f7467fab..7992b68b4 100644 --- a/client/Makefile +++ b/client/Makefile @@ -119,6 +119,7 @@ CORESRCS = uart_posix.c \ util.c \ util_posix.c \ scandir.c \ + crc16.c \ comms.c CMDSRCS = crapto1/crapto1.c \ @@ -143,7 +144,6 @@ CMDSRCS = crapto1/crapto1.c \ mifare/mifarehost.c \ parity.c \ crc.c \ - crc16.c \ crc64.c \ legic_prng.c \ iso15693tools.c \ diff --git a/client/cmdhw.c b/client/cmdhw.c index 655442738..929d37b73 100644 --- a/client/cmdhw.c +++ b/client/cmdhw.c @@ -445,12 +445,13 @@ static int CmdPingNG(const char *Cmd) { PrintAndLogEx(NORMAL, "Pinging with payload len=%d", len); clearCommandBuffer(); UsbCommand resp; - UsbCommand c = {CMD_PING, {len, 0, 0}, {{0}}}; + uint8_t data[USB_CMD_DATA_SIZE] = {0}; + uint16_t cmd = CMD_PING; if (len >= 4) - c.d.asDwords[0] = 0xAABBCCDD; + ((uint32_t *)data)[0]=0xAABBCCDD; if (len >= 8) - c.d.asDwords[(len-1)/4] = 0xDDCCBBAA; - SendCommandNG(&c, len); + ((uint32_t *)data)[(len-1)/4] = 0xDDCCBBAA; + SendCommandNG(cmd, data, len); if (WaitForResponseTimeout(CMD_ACK, &resp, 1000)) { PrintAndLogEx(NORMAL, "PingNG successful"); if (len >= 4) diff --git a/client/comms.c b/client/comms.c index d192b4c90..c6d15087c 100644 --- a/client/comms.c +++ b/client/comms.c @@ -10,6 +10,7 @@ //----------------------------------------------------------------------------- #include "comms.h" +#include "crc16.h" // Serial port that we are communicating with the PM3 on. static serial_port sp = NULL; @@ -25,7 +26,7 @@ static pthread_t USB_communication_thread; // Transmit buffer. static UsbCommand txBuffer; -static uint8_t txBufferNG[sizeof(UsbCommandNGPreamble) + sizeof(UsbCommand) + sizeof(UsbCommandNGPostamble)]; +static uint8_t txBufferNG[USB_PACKET_NG_MAXLEN]; size_t txBufferNGLen; static bool txBuffer_pending = false; static pthread_mutex_t txBufferMutex = PTHREAD_MUTEX_INITIALIZER; @@ -86,10 +87,10 @@ void SendCommand(UsbCommand *c) { //__atomic_test_and_set(&txcmd_pending, __ATOMIC_SEQ_CST); } -void SendCommandNG(UsbCommand *c, size_t len) { +void SendCommandNG(uint16_t cmd, uint8_t* data, size_t len) { #ifdef COMMS_DEBUG - PrintAndLogEx(NORMAL, "Sending %d bytes of payload | cmd %04x\n", len, c->cmd); + PrintAndLogEx(NORMAL, "Sending %d bytes of payload | cmd %04x\n", len, cmd); #endif if (offline) { @@ -102,7 +103,7 @@ void SendCommandNG(UsbCommand *c, size_t len) { } UsbCommandNGPreamble *tx_pre = (UsbCommandNGPreamble *)txBufferNG; - UsbCommandNGPostamble *tx_post = (UsbCommandNGPostamble *)(txBufferNG + sizeof(UsbCommandNGPreamble) + sizeof(UsbCommandNG) + len); + UsbCommandNGPostamble *tx_post = (UsbCommandNGPostamble *)(txBufferNG + sizeof(UsbCommandNGPreamble) + len); pthread_mutex_lock(&txBufferMutex); /** @@ -116,10 +117,12 @@ void SendCommandNG(UsbCommand *c, size_t len) { 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); + tx_pre->cmd = cmd; + memcpy(txBufferNG + sizeof(UsbCommandNGPreamble), data, len); + uint8_t first, second; + compute_crc(CRC_14443_A, txBufferNG, sizeof(UsbCommandNGPreamble) + len, &first, &second); + tx_post->crc = (first << 8) + second; + txBufferNGLen = sizeof(UsbCommandNGPreamble) + len + sizeof(UsbCommandNGPostamble); txBuffer_pending = true; // tell communication thread that a new command can be send diff --git a/client/comms.h b/client/comms.h index d6704617b..f71c82f4c 100644 --- a/client/comms.h +++ b/client/comms.h @@ -52,7 +52,7 @@ bool IsOffline(void); void *uart_receiver(void *targ); void SendCommand(UsbCommand *c); -void SendCommandNG(UsbCommand *c, size_t len); +void SendCommandNG(uint16_t cmd, uint8_t* data, size_t len); void clearCommandBuffer(void); #define FLASHMODE_SPEED 460800 diff --git a/common/cmd.c b/common/cmd.c index 74f8c00f5..d560c60f7 100644 --- a/common/cmd.c +++ b/common/cmd.c @@ -31,7 +31,6 @@ */ #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 = false; diff --git a/include/usb_cmd.h b/include/usb_cmd.h index cbef5aa02..2081d8775 100644 --- a/include/usb_cmd.h +++ b/include/usb_cmd.h @@ -37,24 +37,20 @@ typedef struct { typedef struct { uint32_t magic; - uint32_t length; // length of the variable part, 0 if none. + uint16_t length; // length of the variable part, 0 if none. + uint16_t cmd; } 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; +#define USB_PACKET_NG_MINLEN (sizeof(UsbCommandNGPreamble) + sizeof(UsbCommandNGPostamble)) +#define USB_PACKET_NG_MAXLEN (sizeof(UsbCommandNGPreamble) + USB_CMD_DATA_SIZE + sizeof(UsbCommandNGPostamble)) #ifdef WITH_FPC_HOST // "Session" flag, to tell via which interface next msgs should be sent: USB or FPC USART