From a7773b339539e1b8198edbfef925cfbb702ba10d Mon Sep 17 00:00:00 2001 From: Philippe Teuwen Date: Sat, 20 Apr 2019 03:17:19 +0200 Subject: [PATCH] Introduce reply_mix, to still get some varlen on old API --- armsrc/appmain.c | 2 +- client/comms.c | 37 ++++++++++++++++++++++++++++--------- common/cmd.c | 43 ++++++++++++++++++++++++++++++++----------- common/cmd.h | 1 + include/usb_cmd.h | 3 ++- 5 files changed, 64 insertions(+), 22 deletions(-) diff --git a/armsrc/appmain.c b/armsrc/appmain.c index 3d205ac11..69de9f1a6 100644 --- a/armsrc/appmain.c +++ b/armsrc/appmain.c @@ -1452,7 +1452,7 @@ static void PacketReceived(PacketCommandNG *packet) { if (packet->ng) { reply_ng(CMD_PING, PM3_SUCCESS, packet->data.asBytes, packet->length); } else { - reply_old(CMD_ACK, reply_via_fpc, 0, 0, 0, 0); + reply_mix(CMD_ACK, reply_via_fpc, 0, 0, 0, 0); } break; #ifdef WITH_LCD diff --git a/client/comms.c b/client/comms.c index 9b5dd1c26..59215a299 100644 --- a/client/comms.c +++ b/client/comms.c @@ -335,20 +335,40 @@ __attribute__((force_align_arg_pointer)) bool error = false; if (uart_receive(sp, (uint8_t *)&rx_raw.pre, sizeof(PacketResponseNGPreamble), &rxlen) && (rxlen == sizeof(PacketResponseNGPreamble))) { rx.magic = rx_raw.pre.magic; - rx.length = rx_raw.pre.length; + uint16_t length = rx_raw.pre.length; + rx.ng = rx_raw.pre.ng; rx.status = rx_raw.pre.status; rx.cmd = rx_raw.pre.cmd; if (rx.magic == RESPONSENG_PREAMBLE_MAGIC) { // New style NG reply - if (rx.length > USB_CMD_DATA_SIZE) { - PrintAndLogEx(WARNING, "Received packet frame with incompatible length: 0x%04x", rx.length); + if (length > USB_CMD_DATA_SIZE) { + PrintAndLogEx(WARNING, "Received packet frame with incompatible length: 0x%04x", length); error = true; } - if ((!error) && (rx.length > 0)) { // Get the variable length payload - if ((!uart_receive(sp, (uint8_t *)&rx_raw.data, rx.length, &rxlen)) || (rxlen != rx.length)) { - PrintAndLogEx(WARNING, "Received packet frame error variable part too short? %d/%d", rxlen, rx.length); + if ((!error) && (length > 0)) { // Get the variable length payload + if ((!uart_receive(sp, (uint8_t *)&rx_raw.data, length, &rxlen)) || (rxlen != length)) { + PrintAndLogEx(WARNING, "Received packet frame error variable part too short? %d/%d", rxlen, length); error = true; } else { - memcpy(&rx.data, &rx_raw.data, rx.length); + + + if (rx.ng) { + memcpy(&rx.data, &rx_raw.data, length); + rx.length = length; + } else { + uint64_t arg[3]; + if (length < sizeof(arg)) { + PrintAndLogEx(WARNING, "Received MIX packet frame with incompatible length: 0x%04x", length); + error = true; + } + if (!error) { + memcpy(arg, &rx_raw.data, sizeof(arg)); + rx.oldarg[0] = arg[0]; + rx.oldarg[1] = arg[1]; + rx.oldarg[2] = arg[2]; + memcpy(&rx.data, ((uint8_t *)&rx_raw.data) + sizeof(arg), length - sizeof(arg)); + rx.length = length - sizeof(arg); + } + } } } if (!error) { // Get the postamble @@ -361,7 +381,7 @@ __attribute__((force_align_arg_pointer)) rx.crc = rx_raw.foopost.crc; if (rx.crc != RESPONSENG_POSTAMBLE_MAGIC) { uint8_t first, second; - compute_crc(CRC_14443_A, (uint8_t *)&rx_raw, sizeof(PacketResponseNGPreamble) + rx.length, &first, &second); + compute_crc(CRC_14443_A, (uint8_t *)&rx_raw, sizeof(PacketResponseNGPreamble) + length, &first, &second); if ((first << 8) + second != rx.crc) { PrintAndLogEx(WARNING, "Received packet frame CRC error %02X%02X <> %04X", first, second, rx.crc); error = true; @@ -370,7 +390,6 @@ __attribute__((force_align_arg_pointer)) } if (!error) { // PrintAndLogEx(NORMAL, "Received reply NG full !!"); - rx.ng = true; PacketResponseReceived(&rx); //TODO DOEGOX NG don't send ACK anymore but reply with the corresponding cmd, still things seem to work fine... if (rx.cmd == CMD_ACK) { diff --git a/common/cmd.c b/common/cmd.c index e9736a241..83e48895e 100644 --- a/common/cmd.c +++ b/common/cmd.c @@ -84,7 +84,7 @@ int16_t reply_old(uint64_t cmd, uint64_t arg0, uint64_t arg1, uint64_t arg2, voi return sendlen; } -int16_t reply_ng(uint16_t cmd, int16_t status, uint8_t *data, size_t len) { +static int16_t reply_ng_internal(uint16_t cmd, int16_t status, uint8_t *data, size_t len, bool ng) { PacketResponseNGRaw txBufferNG; size_t txBufferNGLen; // for (size_t i = 0; i < sizeof(txBufferNG); i++) @@ -94,6 +94,7 @@ int16_t reply_ng(uint16_t cmd, int16_t status, uint8_t *data, size_t len) { txBufferNG.pre.magic = RESPONSENG_PREAMBLE_MAGIC; txBufferNG.pre.cmd = cmd; txBufferNG.pre.status = status; + txBufferNG.pre.ng = ng; if (len > USB_CMD_DATA_SIZE) { len = USB_CMD_DATA_SIZE; // overwrite status @@ -135,6 +136,24 @@ int16_t reply_ng(uint16_t cmd, int16_t status, uint8_t *data, size_t len) { return sendlen; } +int16_t reply_ng(uint16_t cmd, int16_t status, uint8_t *data, size_t len) { + return reply_ng_internal(cmd, status, data, len, true); +} + +int16_t reply_mix(uint64_t cmd, uint64_t arg0, uint64_t arg1, uint64_t arg2, void *data, size_t len) { + uint16_t status = PM3_SUCCESS; + uint64_t arg[3] = {arg0, arg1, arg2}; + if (len > USB_CMD_DATA_SIZE - sizeof(arg)) { + len = USB_CMD_DATA_SIZE - sizeof(arg); + status = PM3_EOVFLOW; + } + uint8_t cmddata[USB_CMD_DATA_SIZE]; + memcpy(cmddata, arg, sizeof(arg)); + if (len && data) + memcpy(cmddata + sizeof(arg), data, len); + return reply_ng_internal(cmd, status, cmddata, len + sizeof(arg), false); +} + static int16_t receive_ng_internal(PacketCommandNG *rx, uint32_t read_ng(uint8_t *data, size_t len), bool fpc) { PacketCommandNGRaw rx_raw; size_t bytes = read_ng((uint8_t *)&rx_raw.pre, sizeof(PacketCommandNGPreamble)); @@ -144,26 +163,28 @@ static int16_t receive_ng_internal(PacketCommandNG *rx, uint32_t read_ng(uint8_t return PM3_EIO; rx->magic = rx_raw.pre.magic; rx->ng = rx_raw.pre.ng; - rx->length = rx_raw.pre.length; + uint16_t length = rx_raw.pre.length; rx->cmd = rx_raw.pre.cmd; if (rx->magic == COMMANDNG_PREAMBLE_MAGIC) { // New style NG command - if (rx->length > USB_CMD_DATA_SIZE) + if (length > USB_CMD_DATA_SIZE) return PM3_EOVFLOW; // Get the core and variable length payload - bytes = read_ng((uint8_t *)&rx_raw.data, rx->length); - if (bytes != rx->length) + bytes = read_ng((uint8_t *)&rx_raw.data, length); + if (bytes != length) return PM3_EIO; - if (rx->ng) - memcpy(rx->data.asBytes, rx_raw.data, rx->length); - else { + if (rx->ng) { + memcpy(rx->data.asBytes, rx_raw.data, length); + rx->length = length; + } else { uint64_t arg[3]; - if (rx->length < sizeof(arg)) + if (length < sizeof(arg)) return PM3_EIO; memcpy(arg, rx_raw.data, sizeof(arg)); rx->oldarg[0] = arg[0]; rx->oldarg[1] = arg[1]; rx->oldarg[2] = arg[2]; - memcpy(rx->data.asBytes, rx_raw.data + sizeof(arg), rx->length - sizeof(arg)); + memcpy(rx->data.asBytes, rx_raw.data + sizeof(arg), length - sizeof(arg)); + rx->length = length - sizeof(arg); } // Get the postamble bytes = read_ng((uint8_t *)&rx_raw.foopost, sizeof(PacketCommandNGPostamble)); @@ -173,7 +194,7 @@ static int16_t receive_ng_internal(PacketCommandNG *rx, uint32_t read_ng(uint8_t rx->crc = rx_raw.foopost.crc; if (rx->crc != COMMANDNG_POSTAMBLE_MAGIC) { uint8_t first, second; - compute_crc(CRC_14443_A, (uint8_t *)&rx_raw, sizeof(PacketCommandNGPreamble) + rx->length, &first, &second); + compute_crc(CRC_14443_A, (uint8_t *)&rx_raw, sizeof(PacketCommandNGPreamble) + length, &first, &second); if ((first << 8) + second != rx->crc) return PM3_EIO; } diff --git a/common/cmd.h b/common/cmd.h index ae9d95e46..4c9d6b1d1 100644 --- a/common/cmd.h +++ b/common/cmd.h @@ -41,6 +41,7 @@ int16_t reply_old(uint64_t cmd, uint64_t arg0, uint64_t arg1, uint64_t arg2, void *data, size_t len); int16_t reply_ng(uint16_t cmd, int16_t status, uint8_t *data, size_t len); +int16_t reply_mix(uint64_t cmd, uint64_t arg0, uint64_t arg1, uint64_t arg2, void *data, size_t len); int16_t receive_ng(PacketCommandNG *rx); // Flags to tell where to add CRC on sent replies diff --git a/include/usb_cmd.h b/include/usb_cmd.h index 6ff7f1241..316c99c3c 100644 --- a/include/usb_cmd.h +++ b/include/usb_cmd.h @@ -81,7 +81,8 @@ typedef struct { typedef struct { uint32_t magic; - uint16_t length; // length of the variable part, 0 if none. + uint16_t length : 15; // length of the variable part, 0 if none. + bool ng : 1; int16_t status; uint16_t cmd; } PACKED PacketResponseNGPreamble;