|
23 | 23 | #include "L2CAPSignaling.h"
|
24 | 24 | #include "btct.h"
|
25 | 25 | #include "HCI.h"
|
| 26 | +#include "bitDescriptions.h" |
| 27 | +// #define _BLE_TRACE_ |
26 | 28 |
|
27 | 29 | #define HCI_COMMAND_PKT 0x01
|
28 | 30 | #define HCI_ACLDATA_PKT 0x02
|
@@ -1139,50 +1141,63 @@ void HCIClass::handleEventPkt(uint8_t /*plen*/, uint8_t pdata[])
|
1139 | 1141 | // Load our LTK for this connection.
|
1140 | 1142 | uint8_t peerAddr[7];
|
1141 | 1143 | uint8_t resolvableAddr[6];
|
| 1144 | + uint8_t foundLTK; |
1142 | 1145 | ATT.getPeerAddrWithType(ltkRequest->connectionHandle, peerAddr);
|
1143 | 1146 |
|
1144 |
| - if(ATT.getPeerResolvedAddress(ltkRequest->connectionHandle, resolvableAddr) |
1145 |
| - && !((ATT.getPeerEncryption(ltkRequest->connectionHandle) & PEER_ENCRYPTION::PAIRING_REQUEST)>0)){ |
1146 |
| - _getLTK(resolvableAddr, HCI.LTK); |
| 1147 | + if((ATT.getPeerEncryption(ltkRequest->connectionHandle) & PEER_ENCRYPTION::PAIRING_REQUEST)>0){ |
| 1148 | + // Pairing request - LTK is one in buffer already |
| 1149 | + foundLTK = 1; |
1147 | 1150 | }else{
|
1148 |
| - _getLTK(&peerAddr[1], HCI.LTK); |
| 1151 | + if(ATT.getPeerResolvedAddress(ltkRequest->connectionHandle, resolvableAddr)){ |
| 1152 | + foundLTK = getLTK(resolvableAddr, HCI.LTK); |
| 1153 | + }else{ |
| 1154 | + foundLTK = getLTK(&peerAddr[1], HCI.LTK); |
| 1155 | + } |
1149 | 1156 | }
|
1150 |
| - // } |
| 1157 | + // } //2d |
1151 | 1158 | // Send our LTK back
|
1152 |
| - struct __attribute__ ((packed)) LTKReply |
1153 |
| - { |
1154 |
| - uint16_t connectionHandle; |
1155 |
| - uint8_t LTK[16]; |
1156 |
| - } ltkReply = {0,0}; |
1157 |
| - ltkReply.connectionHandle = ltkRequest->connectionHandle; |
1158 |
| - for(int i=0; i<16; i++) ltkReply.LTK[15-i] = HCI.LTK[i]; |
1159 |
| - int result = sendCommand(OGF_LE_CTL << 10 | LE_COMMAND::LONG_TERM_KEY_REPLY,sizeof(ltkReply), <kReply); |
1160 |
| - |
1161 |
| -#ifdef _BLE_TRACE_ |
1162 |
| - Serial.println("Sending LTK as: "); |
1163 |
| - btct.printBytes(ltkReply.LTK,16); |
1164 |
| -#endif |
1165 |
| - |
1166 |
| - if(result == 0){ |
1167 |
| - struct __attribute__ ((packed)) LTKReplyResult |
| 1159 | + if(foundLTK){ |
| 1160 | + struct __attribute__ ((packed)) LTKReply |
1168 | 1161 | {
|
1169 |
| - uint8_t status; |
1170 | 1162 | uint16_t connectionHandle;
|
1171 |
| - } ltkReplyResult = {0,0}; |
1172 |
| - memcpy(<kReplyResult, _cmdResponse, 3); |
1173 |
| - |
1174 |
| -#ifdef _BLE_TRACE_ |
1175 |
| - Serial.println("LTK send success"); |
1176 |
| - Serial.print("status : "); |
1177 |
| - btct.printBytes(<kReplyResult.status,1); |
1178 |
| - Serial.print("Conn Handle: "); |
1179 |
| - btct.printBytes((uint8_t*)<kReplyResult.connectionHandle,2); |
1180 |
| -#endif |
| 1163 | + uint8_t LTK[16]; |
| 1164 | + } ltkReply = {0,0}; |
| 1165 | + ltkReply.connectionHandle = ltkRequest->connectionHandle; |
| 1166 | + for(int i=0; i<16; i++) ltkReply.LTK[15-i] = HCI.LTK[i]; |
| 1167 | + int result = sendCommand(OGF_LE_CTL << 10 | LE_COMMAND::LONG_TERM_KEY_REPLY,sizeof(ltkReply), <kReply); |
| 1168 | + |
| 1169 | + #ifdef _BLE_TRACE_ |
| 1170 | + Serial.println("Sending LTK as: "); |
| 1171 | + btct.printBytes(ltkReply.LTK,16); |
| 1172 | + #endif |
| 1173 | + |
| 1174 | + if(result == 0){ |
| 1175 | + struct __attribute__ ((packed)) LTKReplyResult |
| 1176 | + { |
| 1177 | + uint8_t status; |
| 1178 | + uint16_t connectionHandle; |
| 1179 | + } ltkReplyResult = {0,0}; |
| 1180 | + memcpy(<kReplyResult, _cmdResponse, 3); |
| 1181 | + |
| 1182 | + #ifdef _BLE_TRACE_ |
| 1183 | + Serial.println("LTK send success"); |
| 1184 | + Serial.print("status : "); |
| 1185 | + btct.printBytes(<kReplyResult.status,1); |
| 1186 | + Serial.print("Conn Handle: "); |
| 1187 | + btct.printBytes((uint8_t*)<kReplyResult.connectionHandle,2); |
| 1188 | + #endif |
| 1189 | + }else{ |
| 1190 | + #ifdef _BLE_TRACE_ |
| 1191 | + Serial.print("Failed to send LTK...: "); |
| 1192 | + btct.printBytes((uint8_t*)&result,2); |
| 1193 | + #endif |
| 1194 | + } |
1181 | 1195 | }else{
|
| 1196 | + /// do LTK rejection |
1182 | 1197 | #ifdef _BLE_TRACE_
|
1183 |
| - Serial.print("Failed to send LTK...: "); |
1184 |
| - btct.printBytes((uint8_t*)&result,2); |
| 1198 | + Serial.println("LTK not found, rejecting"); |
1185 | 1199 | #endif
|
| 1200 | + sendCommand(OGF_LE_CTL << 10 | LE_COMMAND::LONG_TERM_KEY_NEGATIVE_REPLY,2, <kRequest->connectionHandle); |
1186 | 1201 | }
|
1187 | 1202 | break;
|
1188 | 1203 | }
|
@@ -1256,10 +1271,10 @@ void HCIClass::handleEventPkt(uint8_t /*plen*/, uint8_t pdata[])
|
1256 | 1271 |
|
1257 | 1272 |
|
1258 | 1273 | uint8_t Z = 0;
|
1259 |
| - for(int i=0; i<16; i++){ |
1260 |
| - /// TODO: Implement secure random |
1261 |
| - Nb[i] = rand(); //// Should use ESP or ECCx08 |
1262 |
| - } |
| 1274 | + |
| 1275 | + HCI.leRand(Nb); |
| 1276 | + HCI.leRand(&Nb[8]); |
| 1277 | + |
1263 | 1278 | #ifdef _BLE_TRACE_
|
1264 | 1279 | Serial.print("nb: ");
|
1265 | 1280 | btct.printBytes(Nb, 16);
|
@@ -1405,6 +1420,47 @@ int HCIClass::leEncrypt(uint8_t* key, uint8_t* plaintext, uint8_t* status, uint8
|
1405 | 1420 | #endif
|
1406 | 1421 | return res;
|
1407 | 1422 | }
|
| 1423 | +int HCIClass::leRand(uint8_t rand[]){ |
| 1424 | + int res = sendCommand(OGF_LE_CTL << 10 | LE_COMMAND::RANDOM); |
| 1425 | + if(res == 0){ |
| 1426 | + memcpy(rand,_cmdResponse, 8); /// backwards but it's a random number |
| 1427 | + } |
| 1428 | + return res; |
| 1429 | +} |
| 1430 | +int HCIClass::getLTK(uint8_t* address, uint8_t* LTK){ |
| 1431 | + if(_getLTK!=0){ |
| 1432 | + return _getLTK(address, LTK); |
| 1433 | + }else{ |
| 1434 | + return 0; |
| 1435 | + } |
| 1436 | +} |
| 1437 | +int HCIClass::storeIRK(uint8_t* address, uint8_t* IRK){ |
| 1438 | + if(_storeIRK!=0){ |
| 1439 | + return _storeIRK(address, IRK); |
| 1440 | + }else{ |
| 1441 | + return 0; |
| 1442 | + } |
| 1443 | +} |
| 1444 | +int HCIClass::storeLTK(uint8_t* address, uint8_t* LTK){ |
| 1445 | + if(_storeLTK!=0){ |
| 1446 | + return _storeLTK(address, LTK); |
| 1447 | + }else{ |
| 1448 | + return 0; |
| 1449 | + } |
| 1450 | +} |
| 1451 | + |
| 1452 | +/// Stub function to generate parameters for local authreq |
| 1453 | +AuthReq HCIClass::localAuthreq(){ |
| 1454 | + // If get, set, IRK, LTK all set then we can bond. |
| 1455 | + AuthReq local = AuthReq(); |
| 1456 | + if(_storeIRK!=0 && _storeLTK!=0 && _getLTK!=0 && _getIRKs!=0){ |
| 1457 | + local.setBonding(true); |
| 1458 | + } |
| 1459 | + local.setSC(true); |
| 1460 | + local.setMITM(true); |
| 1461 | + local.setCT2(true); |
| 1462 | + return LOCAL_AUTHREQ; |
| 1463 | +} |
1408 | 1464 |
|
1409 | 1465 | void HCIClass::dumpPkt(const char* prefix, uint8_t plen, uint8_t pdata[])
|
1410 | 1466 | {
|
|
0 commit comments