1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
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;
}
|
