summaryrefslogtreecommitdiffstats
path: root/algo_uds_auth.c
diff options
context:
space:
mode:
authorprototux <jason@prototux.net>2020-10-01 21:05:29 +0200
committerprototux <jason@prototux.net>2020-10-01 21:05:29 +0200
commitc871a79b98d13914886a6dbe44472bf1ce18e351 (patch)
tree17018054ab73659f31d7a6c00ca354a357937f94 /algo_uds_auth.c
downloadpsa-uds-nac-master.tar
psa-uds-nac-master.tar.gz
psa-uds-nac-master.tar.bz2
psa-uds-nac-master.tar.lz
psa-uds-nac-master.tar.xz
psa-uds-nac-master.tar.zst
psa-uds-nac-master.zip
Diffstat (limited to 'algo_uds_auth.c')
-rw-r--r--algo_uds_auth.c28
1 files changed, 28 insertions, 0 deletions
diff --git a/algo_uds_auth.c b/algo_uds_auth.c
new file mode 100644
index 0000000..1c4ffba
--- /dev/null
+++ b/algo_uds_auth.c
@@ -0,0 +1,28 @@
+// UDS SA challenge-response implementation
+// This should match the authentication between diagbox and any ECU
+// Thanks to vlud for giving me the will to do it
+#include <inttypes.h>
+
+// Transformation function with PSA not-so-secret sauce
+int16_t transform(uint8_t data_msb, uint8_t data_lsb, uint8_t sec[])
+{
+ int16_t data = (data_msb << 8) | data_lsb;
+ int32_t result = ((data % sec[0]) * sec[2]) - ((data / sec[0]) * sec[1]);
+ if (result < 0)
+ result += (sec[0] * sec[2]) + sec[1];
+ return result;
+}
+
+// Challenge reponse calculation for a given pin and challenge
+// Challenge (seed) is 4 bytes and pin (key) is 2 bytes
+uint32_t compute_response(uint8_t pin[], uint8_t chg[])
+{
+ // Still hardcoded secrets
+ int8_t sec_1[3] = {0xB2, 0x3F, 0xAA};
+ int8_t sec_2[3] = {0xB1, 0x02, 0xAB};
+
+ // Compute each 16b part of the response, with the twist, and return it
+ int16_t res_msb = transform(pin[0], pin[1], sec_1) | transform(chg[0], chg[3], sec_2);
+ int16_t res_lsb = transform(chg[1], chg[2], sec_1) | transform(res_msb>>8, res_msb&0xFF, sec_2);
+ return (msb << 16) | lsb;
+}