static int smp_ah(struct crypto_blkcipher *tfm, u8 irk[16], u8 r[3], u8 res[3])
{
u8 _res[16];
int err;
/* r' = padding || r */
memcpy(_res, r, 3);
memset(_res + 3, 0, 13);
err = smp_e(tfm, irk, _res);
if (err) {
BT_ERR("Encrypt error");
return err;
}
/* The output of the random address function ah is:
* ah(h, r) = e(k, r') mod 2^24
* The output of the security function e is then truncated to 24 bits
* by taking the least significant 24 bits of the output of e as the
* result of ah.
*/
memcpy(res, _res, 3);
return 0;
}
static int smp_s1(const uint8_t *k, uint8_t *r1, uint8_t *r2, uint8_t *out)
{
uint8_t r[16];
int i;
bcopy(r1+8, r, 8);
bcopy(r2+8, r+8, 8);
return smp_e(k, r, out);
}
static int smp_c1(struct crypto_blkcipher *tfm, u8 k[16], u8 r[16],
u8 preq[7], u8 pres[7], u8 _iat, bdaddr_t *ia,
u8 _rat, bdaddr_t *ra, u8 res[16])
{
u8 p1[16], p2[16];
int err;
memset(p1, 0, 16);
/* p1 = pres || preq || _rat || _iat */
swap56(pres, p1);
swap56(preq, p1 + 7);
p1[14] = _rat;
p1[15] = _iat;
memset(p2, 0, 16);
/* p2 = padding || ia || ra */
baswap((bdaddr_t *) (p2 + 4), ia);
baswap((bdaddr_t *) (p2 + 10), ra);
/* res = r XOR p1 */
u128_xor((u128 *) res, (u128 *) r, (u128 *) p1);
/* res = e(k, res) */
err = smp_e(tfm, k, res);
if (err) {
BT_ERR("Encrypt data error");
return err;
}
/* res = res XOR p2 */
u128_xor((u128 *) res, (u128 *) res, (u128 *) p2);
/* res = e(k, res) */
err = smp_e(tfm, k, res);
if (err)
BT_ERR("Encrypt data error");
return err;
}
static int smp_c1(struct crypto_blkcipher *tfm, u8 k[16], u8 r[16],
u8 preq[7], u8 pres[7], u8 _iat, bdaddr_t *ia,
u8 _rat, bdaddr_t *ra, u8 res[16])
{
u8 p1[16], p2[16];
int err;
memset(p1, 0, 16);
swap56(pres, p1);
swap56(preq, p1 + 7);
p1[14] = _rat;
p1[15] = _iat;
memset(p2, 0, 16);
baswap((bdaddr_t *) (p2 + 4), ia);
baswap((bdaddr_t *) (p2 + 10), ra);
u128_xor((u128 *) res, (u128 *) r, (u128 *) p1);
err = smp_e(tfm, k, res);
if (err) {
BT_ERR("Encrypt data error");
return err;
}
u128_xor((u128 *) res, (u128 *) res, (u128 *) p2);
err = smp_e(tfm, k, res);
if (err)
BT_ERR("Encrypt data error");
return err;
}
static int smp_c1(struct crypto_blkcipher *tfm, u8 k[16], u8 r[16],
u8 preq[7], u8 pres[7], u8 _iat, bdaddr_t *ia,
u8 _rat, bdaddr_t *ra, u8 res[16])
{
u8 p1[16], p2[16];
int err;
memset(p1, 0, 16);
/* p1 = pres || preq || _rat || _iat */
p1[0] = _iat;
p1[1] = _rat;
memcpy(p1 + 2, preq, 7);
memcpy(p1 + 9, pres, 7);
/* p2 = padding || ia || ra */
memcpy(p2, ra, 6);
memcpy(p2 + 6, ia, 6);
memset(p2 + 12, 0, 4);
/* res = r XOR p1 */
u128_xor((u128 *) res, (u128 *) r, (u128 *) p1);
/* res = e(k, res) */
err = smp_e(tfm, k, res);
if (err) {
BT_ERR("Encrypt data error");
return err;
}
/* res = res XOR p2 */
u128_xor((u128 *) res, (u128 *) res, (u128 *) p2);
/* res = e(k, res) */
err = smp_e(tfm, k, res);
if (err)
BT_ERR("Encrypt data error");
return err;
}
static int smp_c1(uint8_t *k, uint8_t *r, uint8_t *preq,
uint8_t *pres, uint8_t iat, bdaddr_t *ia,
uint8_t rat, bdaddr_t *ra)
{
uint8_t p1[16];
uint8_t p2[16];
uint8_t tmp[16];
int i;
for(i = 0; i< 7; i++){
p1[i+7] = preq[6-i];
p1[i] = pres[6-i];
}
p1[14] = rat;
p1[15] = iat;
bzero(p2, sizeof(p2));
for(i = 0; i< 6; i++){
p2[i+4] = ia->b[5-i];
p2[i+10] = ra->b[5-i];
}
printf("P1 ");
for(i=0; i<16; i++){
printf("%02x ",p1[i]);
}
printf("\nP2 ");
for(i=0; i<16; i++){
printf("%02x ",p2[i]);
}
printf("\n");
for(i = 0; i < 16; i++){
r[i] = r[i]^p1[i];
}
smp_e(k, r, tmp);
for(i = 0; i < 16; i++){
tmp[i] = tmp[i]^p2[i];
}
smp_e(k, tmp, r);
return 0;
}
static int smp_s1(struct crypto_blkcipher *tfm, u8 k[16],
u8 r1[16], u8 r2[16], u8 _r[16])
{
int err;
/* Just least significant octets from r1 and r2 are considered */
memcpy(_r, r1 + 8, 8);
memcpy(_r + 8, r2 + 8, 8);
err = smp_e(tfm, k, _r);
if (err)
BT_ERR("Encrypt data error");
return err;
}
static int smp_s1(struct crypto_blkcipher *tfm, u8 k[16],
u8 r1[16], u8 r2[16], u8 _r[16])
{
int err;
memcpy(_r, r1 + 8, 8);
memcpy(_r + 8, r2 + 8, 8);
err = smp_e(tfm, k, _r);
if (err)
BT_ERR("Encrypt data error");
return err;
}
