/*
Function that makes a complete ascii frame and sends on uart.
*/
int modbusRS485_send( u_char address, u_char function, u_char* data, uint8_t nbytes)
{
/* initialize variables */
u_char *ptr=NULL, frameLRCByte=0;
u_char rowFrameBuffer[MAX_ASCIIFRAME_SIZE],asciiFrameBuffer[MAX_ASCIIFRAME_SIZE];
int status = MODBUS_SUCCESS;
unsigned int i,endByteStartAdd;
ptz_write(RS485_DEVICE_WRITE_MODE);
usleep(10000);
if((address == 0x00) || (function== 0x00))
{
printf("modbusRS485_init:Wrong input data \n");
status = EMODBUS_ADDR_FUNC;
}
else
{
rowFrameBuffer[ADDR_INDEX] = address; //address
rowFrameBuffer[FUNC_INDEX] = function; //command
if(data == NULL)
rowFrameBuffer[DATALEN_INDEX] = 0x00;
else {
rowFrameBuffer[DATALEN_INDEX] = nbytes;
str_copy(&rowFrameBuffer[DATA_INDEX],data, nbytes);
}
rowFrameBuffer[ nbytes + DATA_INDEX ] = compute_LRCByte(&rowFrameBuffer[ADDR_INDEX], nbytes+ DATA_INDEX );
/*all parameter set in rowdatabuffer */
asciiFrameBuffer[START_BYTE_OFFSET] = START_BYTE; // start byte - ":"
nbytes= nbytes + DATA_INDEX + LRC_BYTE_SIZE; // Adding add+func+LRCcode with data
bytes2ascii(rowFrameBuffer, &asciiFrameBuffer[1],nbytes); //
endByteStartAdd = ASCII_CODE_SIZE * (nbytes);
asciiFrameBuffer[++endByteStartAdd] = END_BYTE1;
asciiFrameBuffer[++endByteStartAdd] = END_BYTE2;
status = uart_write(rs485port,asciiFrameBuffer,++endByteStartAdd);
if(status != MODBUS_SUCCESS){
printf("modbusRS485_send: uart_write failed \n");
}
usleep(13000);
// ptz_write(RS485_DEVICE_READ_MODE);
}
#if MODBUS_DEBUG
ascii_dump(asciiFrameBuffer,endByteStartAdd );
#endif
return status;
}
ReturnValue BleApiTest_Enroll(pBleDevice dev, int expectedSW12)
{
uint64_t t = dev->TimeMs();
ReturnValue retval;
int i;
unsigned char reply[2048];
unsigned int replyLength = sizeof(reply);
unsigned char request[256];
unsigned int requestlen;
unsigned char replyCmd;
memset(reply, 0, sizeof(reply));
/* generate appid and nonce */
for (i = 0; i < sizeof(regReq.appId); i++)
regReq.appId[i] = (rand() & 0xFF);
for (i = 0; i < sizeof(regReq.nonce); i++)
regReq.nonce[i] = (rand() & 0xFF);
/* prepare register request */
request[0] = 0x00;
request[1] = 0x01;
request[2] = 0x00;
request[3] = 0x00;
request[4] = 0x00;
request[5] = 0x00;
request[6] = sizeof(regReq.nonce) + sizeof(regReq.appId);
memcpy(request + 7, regReq.nonce, sizeof(regReq.nonce));
memcpy(request + 7 + sizeof(regReq.nonce), regReq.appId,
sizeof(regReq.appId));
requestlen = 7 + sizeof(regReq.nonce) + sizeof(regReq.appId);
request[requestlen++] = 0x00;
request[requestlen++] = 0x00;
/* write command */
retval =
dev->CommandWrite(FIDO_BLE_CMD_MSG, request, requestlen, &replyCmd,
reply, &replyLength);
CHECK_EQ(retval, ReturnValue::BLEAPI_ERROR_SUCCESS);
if (expectedSW12 != FIDO_RESP_SUCCESS) {
CHECK_EQ(expectedSW12, bytes2short(reply, replyLength - 2), "Returned error does not match expected value.");
CHECK_EQ(replyLength, 2, "Returned value does not match expected length.");
return ReturnValue::BLEAPI_ERROR_SUCCESS;
}
/* check reply */
CHECK_EQ(replyCmd, FIDO_BLE_CMD_MSG, "Reply is not a FIDO_BLE_CMD_MSG (0x83)");
CHECK_EQ(FIDO_RESP_SUCCESS, bytes2short(reply, replyLength - 2), "Status code is not FIDO_RESP_SUCCESS (0x9000)");
CHECK_NE(replyLength, 2, "Reply length is only status code.");
CHECK_LE(replyLength - 2, sizeof(U2F_REGISTER_RESP), "Returned register response does not match expected length.");
memcpy(®Rsp, reply, replyLength - 2);
CHECK_EQ(regRsp.registerId, U2F_REGISTER_ID, "Register ID is not 0x05");
CHECK_EQ(regRsp.pubKey.format, UNCOMPRESSED_POINT, "Public Key format is not uncompressed point.");
INFO << "Enroll: " << (replyLength -
2) << " bytes in " << ((float)(dev->TimeMs() -
t)) /
1000.0 << "s";
// Check crypto of enroll response.
std::string cert;
CHECK_EQ(getCertificate(regRsp, &cert), true, "Cannot extract certificate.");
INFO << "cert: " << bytes2ascii(cert);
std::string pk;
CHECK_EQ(getSubjectPublicKey(cert, &pk), true, "Cannot extract public key.");
INFO << "pk : " << bytes2ascii(pk);
std::string sig;
CHECK_EQ(getSignature(regRsp, static_cast<int>(cert.size()), &sig), true, "Cannot extract signature.");
INFO << "sig : " << bytes2ascii(sig);
// Parse signature into two integers.
p256_int sig_r, sig_s;
CHECK_EQ(1, dsa_sig_unpack((uint8_t *) (sig.data()), static_cast<int>(sig.size()),
&sig_r, &sig_s), "Cannot unpack signature");
// Compute hash as integer.
const uint8_t *hash;
p256_int h;
SHA256_CTX sha;
SHA256_init(&sha);
uint8_t rfu = 0;
SHA256_update(&sha, &rfu, sizeof(rfu)); // 0x00
SHA256_update(&sha, regReq.appId, sizeof(regReq.appId)); // O
SHA256_update(&sha, regReq.nonce, sizeof(regReq.nonce)); // d
SHA256_update(&sha, regRsp.keyHandleCertSig, regRsp.keyHandleLen); // hk
SHA256_update(&sha, ®Rsp.pubKey, sizeof(regRsp.pubKey)); // pk
hash = SHA256_final(&sha);
p256_from_bin(hash, &h);
INFO << "hash : " << bytes2ascii((char *)hash, 32);
// Parse subject public key into two integers.
CHECK_EQ(pk.size(), P256_POINT_SIZE, "Public key does not match P256 point size.");
p256_int pk_x, pk_y;
p256_from_bin((uint8_t *) pk.data() + 1, &pk_x);
p256_from_bin((uint8_t *) pk.data() + 1 + P256_SCALAR_SIZE, &pk_y);
// Verify signature.
CHECK_EQ(1, p256_ecdsa_verify(&pk_x, &pk_y, &h, &sig_r, &sig_s), "Signature does not match.");
return ReturnValue::BLEAPI_ERROR_SUCCESS;
}
