int QSmartCardPrivate::rsa_sign( int type, const unsigned char *m, unsigned int m_len,
unsigned char *sigret, unsigned int *siglen, const RSA *rsa )
{
if( type != NID_md5_sha1 && m_len != 36 )
return 0;
QSmartCardPrivate *d = (QSmartCardPrivate*)RSA_get_app_data( rsa );
if ( !d )
return 0;
try
{
ByteVec vec = d->card->sign( ByteVec( m, m + m_len ), EstEidCard::SSL, EstEidCard::AUTH );
if( vec.size() == 0 )
return 0;
*siglen = (unsigned int)vec.size();
memcpy( sigret, &vec[0], vec.size() );
return 1;
}
catch( const std::runtime_error &e )
{
qDebug() << Q_FUNC_INFO << e.what();
}
return 0;
}
string CTAPIManager::getReaderState(uint index)
{
string retval;
ensureReaders(index);
cPort *dri = mPorts[index];
// byte cmd[] = {0x20,INS_GETSTATUS,0x00,0x80,0x00}; //GET STATUS, CT, all ICC status
// byte cmd[] = {0x20,INS_GETSTATUS,0x00,0x81,0x00}; //GET STATUS, CT, functional spec
byte cmd[] = {0x20,INS_GETSTATUS,0x01,0x80,0x00}; //GET STATUS, ICC0, ICC0 status
initclose _i(dri);
ByteVec resp;
dri->performCmd(CTDAD_CT,MAKEVECTOR(cmd),resp,mLogger);
if(resp.size() != 3) {
if (resp.size() == 2) //workaround for openct
resp.insert(resp.begin(),0x80);
else
throw CTAPIError("getReaderState0",0,resp.size(),0,0);
}
if (resp[0] != 0x80)
throw CTAPIError("getReaderState1",0,resp.size(),resp[0],0);
byte statusICC0 = resp[2];
if (statusICC0 & 0x01)
retval+= "PRESENT|";
else
retval+= "EMPTY|";
if ((statusICC0 & 0x6) == 0x2)
retval+= "UNPOWERED|";
if ((statusICC0 & 0x6) == 0x4)
retval+= "INUSE|";
if (retval.length() > 0 )
retval = retval.substr(0,retval.length()-1);
return retval;
}
int RWMemGetData(SDL_RWops *ops, void *buffer)
{
ByteVec *v = getRWPrivate(ops);
if (buffer)
memcpy(buffer, &(*v)[0], v->size());
return v->size();
}
CardBase::FCI EstEidCard::parseFCI(ByteVec fci)
{
mManager.writeLog("[%s:%d]", __FUNCTION__, __LINE__);
ByteVec tag;
FCI tmp;
tmp.fileID = 0;
if (fci.size() < 0x0B ||
(fci[0] != tagFCP && fci[0] != tagFCI) ||
fci.size()-2 != fci[1] )
throw CardDataError("invalid fci record");
/* if (getCardVersion() == VER_3_0)
fci = ByteVec(fci.begin()+ 4, fci.end());
else
*/
fci = ByteVec(fci.begin()+ 2, fci.end());
tag = getTag(0x83, 2, fci);
if (tag.size() != 2)
throw CardDataError("file name record invalid length, not two bytes");
tmp.fileID = MAKEWORD(tag[1],tag[0]);
tag = getTag(0x82, 0, fci);
switch (tag[0] & 0x3F) {
case 0x38: //DF
case 0x01: //binary
if (tag.size() != 1)
throw CardDataError("linear variable file descriptor not 1 bytes long");
tag = getTag(0x85,2,fci);
tmp.fileLength = MAKEWORD(tag[1],tag[0]);
break;
case 0x02:
//linear variable
case 0x04:
if (tag.size() != 5)
throw CardDataError("linear variable file descriptor not 5 bytes long");
tmp.recMaxLen = MAKEWORD( tag[0x03], tag[0x02] );
tmp.recCount = tag[0x04];
tmp.fileLength = 0;
break;
default:
throw CardDataError("invalid filelen record, unrecognized tag");
}
return tmp;
}
bool EstEidCard::getKeyUsageCounters(dword &authKey,dword &signKey)
{
Transaction _m(mManager,mConnection);
selectMF(true);
selectDF(FILEID_APP,true);
selectEF(FILEID_KEYPOINTER,true);
ByteVec keyRec = readRecord(1);
if (keyRec.size() != 0x15)
throw CardDataError("key ptr len is not 0x15");
byte UsedKey = 0;
if (keyRec[0x9]== 0x12 && keyRec[0xA] == 0x00 ) UsedKey++;
FCI fileInfo = selectEF(0x0013);
if (fileInfo.recCount < 4)
throw CardDataError("key info file 0x13 should have 4 records");
ByteVec authData = readRecord(UsedKey + 3);
ByteVec signData = readRecord(UsedKey + 1);
ByteVec aTag = getTag(0x91,3,authData);
ByteVec sTag = getTag(0x91,3,signData);
authKey = 0xFFFFFF - ((aTag[0] << 16) + (aTag[1] << 8) + aTag[2]);
signKey = 0xFFFFFF - ((sTag[0] << 16) + (sTag[1] << 8) + sTag[2]);
return true;
}
void CTDriver::CTPort::performCmd(byte target,ByteVec cmd,ByteVec &resp,
std::ostream *mLogger,bool consumeStatus ) {
byte response[512];
ushort lenr = sizeof(response);
byte sad = CTSAD_HOST;
dump(mLogger,string("CTAPI->cmd ") + (target == CTDAD_CT ? "CT " : "ICC " ),cmd);
byte res = dri->pCTData(mCtn,&target,&sad,
ushort(cmd.size()),&cmd[0],&lenr,response);
if (res!=CTERR_OK || lenr < 2)
throw CTAPIError("performCmd",res,lenr,0,0);
resp.resize(0);
resp.insert(resp.end(),response,response+lenr);
dump(mLogger,"CTAPI<-resp ",resp);
if (consumeStatus) {
byte SW1 = resp[ lenr - 2 ];
byte SW2 = resp[ lenr - 1 ];
if (SW1 != 0x90) {
if (sad == CTDAD_CT) {
if (SW1==0x64 && ( SW2 == 0x00 || SW2 == 0x01 || SW2 == 0x02) ) {
throw AuthError(SW1,SW2);
}
}
throw CardError(SW1,SW2);
}
resp.resize(lenr - 2);
}
}
//"icc status data object" . ct-bcs"
string CTAPIManager::getATRHex(uint index)
{
string retval;
ensureReaders(index);
cPort *dri = mPorts[index];
byte cmd[] = {0x20,INS_GETSTATUS,0x00,0x80,0x00};
initclose _i(dri);
ByteVec resp;
dri->performCmd(CTDAD_CT,MAKEVECTOR(cmd),resp,mLogger);
if (resp[2] == 0) {
return "";
}
byte cmdReq[] = {0x20,INS_REQICC,0x01,0x01,0x00}; // req ICC1, return ATR
dri->performCmd(CTDAD_CT,MAKEVECTOR(cmdReq),resp,mLogger);
std::ostringstream buf;
buf << "";
for(uint i=0;i < resp.size();i++)
buf << std::setfill('0') << std::setw(2) <<std::hex <<
(short) resp[i];
retval = buf.str();
return retval;
}
bool EstEidCard::changePin_internal(
PinType pinType,const PinString &newPin,const PinString &oldPin,bool useUnblockCmd ) {
mManager.writeLog("[%s:%d]", __FUNCTION__, __LINE__);
byte cmdChangeCmd[] = {0x00,0x24,0x00};
bool doSecure = false;
//mManager.beginTransaction(mConnection);
if (useUnblockCmd) cmdChangeCmd[1]= 0x2C;
ByteVec cmd(MAKEVECTOR(cmdChangeCmd));
cmd.push_back((byte)pinType);
size_t oldPinLen,newPinLen;
if (newPin.length() < 4 || oldPin.length() < 4 ) {
if (!mConnection->isSecure() )
{
mManager.endTransaction(mConnection);
throw std::runtime_error("bad pin length");
}
doSecure = true;
} else {
oldPinLen = oldPin.length();
newPinLen = newPin.length();
ByteVec catPins;
catPins.resize(oldPinLen + newPinLen);
copy(oldPin.begin(), oldPin.end(), catPins.begin());
copy(newPin.begin(), newPin.end(), catPins.begin() + oldPin.length());
cmd.push_back(LOBYTE(catPins.size()));
cmd.insert(cmd.end(),catPins.begin(),catPins.end());
}
try
{
if (doSecure)
executePinChange(cmd,0,0);
else
execute(cmd);
}
catch(AuthError &ae)
{
mManager.endTransaction(mConnection);
throw AuthError(ae);
}
catch(CardError &e)
{
mManager.endTransaction(mConnection);
if (e.SW1 == 0x63)
throw AuthError(e);
else if (useUnblockCmd && e.SW1==0x6a && e.SW2 == 0x80 ) //unblocking, repeating old pin
throw AuthError(e.SW1,e.SW2,true);
else
throw e;
}
mManager.endTransaction(mConnection);
return true;
}
ByteVec EstEidCard::readEFAndTruncate(unsigned int fileLen)
{
ByteVec ret = readEF(fileLen);
if (ret.size() > 128) { //assume ASN sequence encoding with 2-byte length
size_t realSize = ret[2] * 256 + ret[3] + 4;
ret.resize(realSize);
}
return ret;
}
ByteVec EstEidCard::calcSign_internal(AlgType type,KeyType keyId,ByteVec hash,bool withOID)
{
byte signCmdSig[] = {0x00,0x2A,0x9E,0x9A};
byte signCmdAut[] = {0x00,0x88,0x00,0x00};
byte hashHdMD5[] = {0x30,0x20,0x30,0x0C,0x06,0x08,0x2A,0x86,0x48,0x86,0xF7,0x0D,0x02,0x05,0x05,0x00,0x04,0x10};
byte hashHdSHA1[] = {0x30,0x21,0x30,0x09,0x06,0x05,0x2B,0x0E,0x03,0x02,0x1A,0x05,0x00,0x04,0x14};
ByteVec cmd,header;
if (keyId == 0 )
cmd = MAKEVECTOR(signCmdAut);
else
cmd = MAKEVECTOR(signCmdSig);
if (withOID) {
switch(type) {
case MD5:
header = MAKEVECTOR(hashHdMD5);
break;
case SHA1:
header = MAKEVECTOR(hashHdSHA1);
break;
default:
throw std::runtime_error("cannot calculate SSL signature with OID");
}
cmd.push_back((byte) (header.size() + hash.size()));
cmd.insert(cmd.end(), header.begin(), header.end());
}
else
cmd.push_back((byte)hash.size());
setSecEnv(1);
cmd.insert(cmd.end(), hash.begin(), hash.end());
ByteVec result;
try {
result = execute(cmd);
} catch(CardError e) {
if (e.SW1 == 0x69 && (e.SW2 == 0x82 || e.SW2 == 0x00 || e.SW2 == 0x85 ))
throw AuthError(e);
throw e;
}
return result;
}
ByteVec EstEidCard::readEFAndTruncate(unsigned int fileLen)
{
mManager.writeLog("[%s:%d]", __FUNCTION__, __LINE__);
ByteVec ret = readEF(fileLen);
if (ret.size() > 128) { //assume ASN sequence encoding with 2-byte length
size_t realSize = ret[2] * 256 + ret[3] + 4;
ret.resize(realSize);
}
return ret;
}
void EstEIDKeyHandle::generateSignature(const Context &context,
CSSM_ALGORITHMS signOnly, const CssmData &input, CssmData &signature) {
FLOG;
_log("EstEIDKeyHandle::generateSignature alg: %u signOnly: %u",
context.algorithm(), signOnly);
IFDUMPING("esteid.tokend", context.dump("signature context"));
if (context.type() != CSSM_ALGCLASS_SIGNATURE)
CssmError::throwMe(CSSMERR_CSP_INVALID_CONTEXT);
if (context.algorithm() != CSSM_ALGID_RSA)
CssmError::throwMe(CSSMERR_CSP_INVALID_ALGORITHM);
if (signOnly == CSSM_ALGID_NONE) {
// Special case used by SSL it's an RSA signature, without the ASN1
// stuff
_log("SSL signature request");
}
else
CssmError::throwMe(CSSMERR_CSP_INVALID_DIGEST_ALGORITHM);
#if !defined(NDEBUG)
context.dump("signature context");
#endif
uint32 padding = CSSM_PADDING_PKCS1;
context.getInt(CSSM_ATTRIBUTE_PADDING, padding);
if (padding != CSSM_PADDING_PKCS1)
CssmError::throwMe(CSSMERR_CSP_INVALID_ATTR_PADDING);
try {
ByteVec result = mToken.getCard().sign(ByteVec(input.Data, input.Data + input.Length), EstEIDManager::SSL, EstEIDManager::AUTH);
unsigned char *outputData = reinterpret_cast<unsigned char *>(malloc(result.size()));
memcpy(outputData, &result[0], result.size());
signature.Data = outputData;
signature.Length = result.size();
} catch(std::runtime_error &err) {
_log("exception while signing");
CssmError::throwMe(CSSMERR_CSP_FUNCTION_FAILED);
}
}
string CTAPIManager::getReaderName(uint index)
{
string retval;
ensureReaders(index);
cPort *dri = mPorts[index];
byte cmd[] = {0x20,INS_GETSTATUS,0x00,0x46,0x00};
initclose _i(dri);
ByteVec resp;
dri->performCmd(CTDAD_CT,MAKEVECTOR(cmd),resp,mLogger);
if (resp[0] != 0x46) {
if (resp.size() != 2) { //openCT
throw CTAPIError("getReaderName",0,resp.size(),resp[0],resp[1]);
}
return "opensc?";
}
resp.erase(resp.begin(),resp.begin()+2);
retval.resize(resp.size());
copy(resp.begin(),resp.end(),retval.begin());
return retval;
}
bool writeFile(const string &fileName, const ByteVec &data)
{
FILE *outFile = fopen(fileName.c_str(), "wb");
if (!outFile) {
return false;
}
int count = fwrite(&data[0], //.begin(),
sizeof(unsigned char),
data.size(),
outFile);
if (count != (int)data.size()) {
fclose(outFile);
return false;
}
fclose(outFile);
return true;
}
bool EstEidCard::changePin_internal(
PinType pinType,PinString newPin,PinString oldPin,bool useUnblockCmd ) {
byte cmdChangeCmd[] = {0x00,0x24,0x00};
bool doSecure = false;
if (useUnblockCmd) cmdChangeCmd[1]= 0x2C;
ByteVec cmd(MAKEVECTOR(cmdChangeCmd));
cmd.push_back((byte)pinType);
size_t oldPinLen,newPinLen;
if (newPin.length() < 4 || oldPin.length() < 4 ) {
if (!mConnection->isSecure() )
throw std::runtime_error("bad pin length");
// FIXME: This is probably broken on PC/SC readers and
// it is not known to work on CT-API either
oldPinLen = (oldPin[0] - '0') * 10 + oldPin[1] - '0';
newPinLen = (newPin[0] - '0') * 10 + newPin[1]- '0';
oldPin = PinString(oldPinLen,'0');
newPin = PinString(newPinLen,'0');
doSecure = true;
} else {
oldPinLen = oldPin.length();
newPinLen = newPin.length();
}
ByteVec catPins;
catPins.resize(oldPinLen + newPinLen);
copy(oldPin.begin(), oldPin.end(), catPins.begin());
copy(newPin.begin(), newPin.end(), catPins.begin() + oldPin.length());
cmd.push_back(LOBYTE(catPins.size()));
cmd.insert(cmd.end(),catPins.begin(),catPins.end());
try {
if (doSecure)
executePinChange(cmd,oldPinLen,newPinLen);
else
execute(cmd,true);
} catch(AuthError &ae) {
throw AuthError(ae);
} catch(CardError &e) {
if (e.SW1 == 0x63)
throw AuthError(e);
else if (useUnblockCmd && e.SW1==0x6a && e.SW2 == 0x80 ) //unblocking, repeating old pin
throw AuthError(e.SW1,e.SW2,true);
else
throw e;
}
return true;
}
void EstEidCardMaintainer::performGenerateNewKeys() {
card.readCardID();
card.selectEF(EstEidCard::FILEID_KEYPOINTER);
ByteVec keyRec = card.readRecord(1);
if (keyRec.size() != 0x15)
throw CardDataError("key ptr len is not 0x15");
ByteVec authPtr(keyRec.begin() + 0x09, keyRec.begin() + 0x0A);
ByteVec signPtr(keyRec.begin() + 0x13, keyRec.begin() + 0x14);
card.selectMF();
card.setSecEnv(3);
card.selectDF(EstEidCard::FILEID_APP);
card.setSecEnv(3);
/*CardBase::FCI fileInfo = */card.selectEF(0x0013);
card.readEF(1);
}
void CTAPIManager::beginTransaction(ConnectionBase *c)
{
CTAPIConnection *conn = (CTAPIConnection *) c;
byte cmdReq[] = {0x20,INS_REQICC,0x01,0x00,0x00}; //req ICC1, ATR return
ByteVec resp;
int retry = 1;
byte SW1;
do {
conn->dri->performCmd(CTDAD_CT,MAKEVECTOR(cmdReq),resp,mLogger,false);
SW1 = *(resp.end() - 2);
} while(retry-- && SW1 == 0x64); //retry once
if (SW1 == 0x62 ) {//already present, SPR reader does that
conn->mForceT0 = true; //and works only with T0
return;
}
if (SW1 != 0x90)
throw CTAPIError("beginTransaction",0,resp.size(),SW1,0);
conn->isT1 = *(--resp.end()) != 0;
}
bool EstEidCard::getKeyUsageCounters(dword &authKey,dword &signKey)
{
mManager.writeLog("[%s:%d]", __FUNCTION__, __LINE__);
mManager.beginTransaction(mConnection);
try
{
selectMF(true);
selectDF(FILEID_APP,true);
selectEF(FILEID_KEYPOINTER,true);
ByteVec keyRec = readRecord(1);
if (keyRec.size() != 0x15)
{
mManager.endTransaction(mConnection);
throw CardDataError("key ptr len is not 0x15");
}
byte UsedKey = 0;
if (keyRec[0x9]== 0x12 && keyRec[0xA] == 0x00 ) UsedKey++;
FCI fileInfo = selectEF(0x0013);
if (fileInfo.recCount < 4)
{
mManager.endTransaction(mConnection);
throw CardDataError("key info file 0x13 should have 4 records");
}
ByteVec authData = readRecord(UsedKey + 3);
ByteVec signData = readRecord(UsedKey + 1);
ByteVec aTag = getTag(0x91,3,authData);
ByteVec sTag = getTag(0x91,3,signData);
authKey = 0xFFFFFF - ((aTag[0] << 16) + (aTag[1] << 8) + aTag[2]);
signKey = 0xFFFFFF - ((sTag[0] << 16) + (sTag[1] << 8) + sTag[2]);
mManager.endTransaction(mConnection);
return true;
}
catch(...)
{
mManager.endTransaction(mConnection);
throw CardDataError("Failed to read data from card");
}
}
static size_t SDL_RWopsWrite(SDL_RWops *ops, const void *buffer, size_t size, size_t num)
{
ByteVec *v = getRWPrivate(ops);
size_t writeBytes = size * num;
if (writeBytes == 1)
{
char byte = *static_cast<const char*>(buffer);
v->push_back(byte);
return 1;
}
size_t writeInd = v->size();
v->resize(writeInd + writeBytes);
memcpy(&(*v)[writeInd], buffer, writeBytes);
return num;
}
MojErr MojDbLevelQuery::seekImpl(const ByteVec& key, bool desc, bool& foundOut)
{
if (key.empty()) {
// if key is empty, seek to beginning (or end if desc)
MojErr err = getKey(foundOut, SeekEmptyFlags[desc]);
MojErrCheck(err);
} else {
// otherwise seek to the key
MojErr err = m_key.fromBytes(key.begin(), key.size());
MojErrCheck(err);
err = getKey(foundOut, SeekFlags);
MojErrCheck(err);
// if descending, skip the first result (which is outside the range)
if (desc) {
err = next(foundOut);
MojErrCheck(err);
}
}
return MojErrNone;
}
ByteVec EstEidCard::RSADecrypt_internal(ByteVec cipher)
{
byte modEnv1[] = {0x00,0x22,0x41,0xA4,0x02,0x83,0x00};
byte modEnv2[] = {0x00,0x22,0x41,0xB6,0x02,0x83,0x00};
byte modEnv0[] = {0x00,0x22,0x41,0xB8,0x05,0x83,0x03,0x80};
selectMF(true);
selectDF(FILEID_APP,true);
selectEF(FILEID_KEYPOINTER,true);
ByteVec keyRec = readRecord(1);
if (keyRec.size() != 0x15)
throw CardDataError("key ptr len is not 0x15");
ByteVec cmd(MAKEVECTOR(modEnv0));
cmd.insert(cmd.end(),&keyRec[0x9],&keyRec[0xB]);
setSecEnv(6);
execute(MAKEVECTOR(modEnv1),true);
execute(MAKEVECTOR(modEnv2),true);
execute(cmd,true);
byte decrpt[] = {0x00,0x2A,0x80,0x86,0x81,0x00};
ByteVec decCmd(MAKEVECTOR(decrpt));
decCmd.insert(decCmd.end(),cipher.begin(),cipher.end());
ByteVec result;
try {
result = execute(decCmd,false);
} catch(CardError e) {
/* if (e.SW1 == 0x69 && e.SW2 == 0x88 ) {
reverse(decCmd.begin() + 6, decCmd.end());
result = execute(decCmd,false);
return result;
}*/
if (e.SW1 == 0x69 && (e.SW2 == 0x82 || e.SW2 == 0x00 || e.SW2 == 0x88 || e.SW2 == 0x85
/*|| e.SW1 == 0x64 */|| e.SW1 == 0x6B ))
throw AuthError(e);
throw e;
}
return result;
}
int MojDbIsamQuery::compareKey(const ByteVec& key)
{
return MojLexicalCompare(m_keyData, m_keySize, key.begin(), key.size());
}
void read(boost::optional<mil_msgs::VelocityMeasurements> &res, boost::optional<mil_msgs::RangeStamped> &height_res)
{
res = boost::none;
height_res = boost::none;
if (!p.is_open()) // Open serial port if closed
{
open();
return;
}
ByteVec ensemble;
ensemble.resize(4);
// Header ID
if (!read_byte(ensemble[0]))
return;
if (ensemble[0] != 0x7F)
return;
ros::Time stamp = ros::Time::now();
// Data Source ID
if (!read_byte(ensemble[1]))
return;
if (ensemble[1] != 0x7F)
return;
// Size low
if (!read_byte(ensemble[2]))
return;
// Size high
if (!read_byte(ensemble[3]))
return;
uint16_t ensemble_size = getu16le(ensemble.data() + 2);
ensemble.resize(ensemble_size);
for (int i = 4; i < ensemble_size; i++)
{
if (!read_byte(ensemble[i]))
return;
}
uint16_t checksum = 0;
BOOST_FOREACH (uint16_t b, ensemble)
checksum += b;
uint16_t received_checksum;
if (!read_short(received_checksum))
return;
if (received_checksum != checksum)
{
ROS_ERROR_THROTTLE(0.5, "DVL: invalid ensemble checksum. received: %i calculated: %i size: %i", received_checksum,
checksum, ensemble_size);
return;
}
if (ensemble.size() < 6)
return;
for (int dt = 0; dt < ensemble[5]; dt++)
{
int offset = getu16le(ensemble.data() + 6 + 2 * dt);
if (ensemble.size() - offset < 2)
continue;
// Three modes, encoded by the section_id: Bottom Track High Resolution Velocity
// Bottom Track, Bottom Track Range
uint16_t section_id = getu16le(ensemble.data() + offset);
std::vector<double> correlations(4, nan(""));
if (section_id == 0x5803) // Bottom Track High Resolution Velocity
{
if (ensemble.size() - offset < 2 + 4 * 4)
continue;
res = boost::make_optional(mil_msgs::VelocityMeasurements());
res->header.stamp = stamp;
std::vector<geometry_msgs::Vector3> dirs;
{
double tilt = 30 * boost::math::constants::pi<double>() / 180;
double x = sin(tilt);
double z = cos(tilt);
dirs.push_back(mil_tools::make_xyz<geometry_msgs::Vector3>(-x, 0, -z));
dirs.push_back(mil_tools::make_xyz<geometry_msgs::Vector3>(+x, 0, -z));
dirs.push_back(mil_tools::make_xyz<geometry_msgs::Vector3>(0, +x, -z));
dirs.push_back(mil_tools::make_xyz<geometry_msgs::Vector3>(0, -x, -z));
}
// Keep track of which beams didn't return for logging
std::vector<size_t> invalid_beams;
invalid_beams.reserve(4);
for (int i = 0; i < 4; i++)
{
mil_msgs::VelocityMeasurement m;
m.direction = dirs[i];
int32_t vel = gets32le(ensemble.data() + offset + 2 + 4 * i);
m.velocity = -vel * .01e-3;
if (vel == -3276801) // -3276801 indicates no data
{
invalid_beams.push_back(i + 1);
m.velocity = nan("");
}
res->velocity_measurements.push_back(m);
}
// Report a list of invalid beams
if (invalid_beams.size() > 0)
{
std::string to_log{ "DVL: didn't return bottom velocity for beam(s): " };
for (auto beam : invalid_beams)
to_log += std::to_string(beam) + " ";
ROS_ERROR_THROTTLE(0.5, "%s", to_log.c_str());
}
}
else if (section_id == 0x0600) // Bottom Track
{
for (int i = 0; i < 4; i++)
correlations[i] = *(ensemble.data() + offset + 32 + i);
}
else if (section_id == 0x5804) // Bottom Track Range
{
if (ensemble.size() - offset < 2 + 4 * 3)
continue;
if (gets32le(ensemble.data() + offset + 10) <= 0)
{
ROS_ERROR_THROTTLE(0.5, "%s", "DVL: didn't return height over bottom");
continue;
}
height_res = boost::make_optional(mil_msgs::RangeStamped());
height_res->header.stamp = stamp;
height_res->range = gets32le(ensemble.data() + offset + 10) * 0.1e-3;
}
if (res)
{
for (int i = 0; i < 4; i++)
{
res->velocity_measurements[i].correlation = correlations[i];
}
}
}
}
static Sint64 SDL_RWopsSize(SDL_RWops *ops)
{
ByteVec *v = getRWPrivate(ops);
return v->size();
}
void QSmartCard::run()
{
while( !d->terminate )
{
if( d->m.tryLock() )
{
// Get list of available cards
QMap<QString,std::string> cards;
QStringList readers;
try
{
EstEIDManager *card = new EstEIDManager();
std::vector<Token> list = card->getTokenList();
for( std::vector<Token>::const_iterator i = list.begin(); i != list.end(); ++i )
{
qDebug() << i->first.c_str() << i->second.c_str();
readers << i->first.c_str();
if( !i->second.empty() )
cards[i->second.c_str()] = i->first.c_str();
}
delete card;
}
catch( const std::runtime_error &e )
{
qDebug() << Q_FUNC_INFO << e.what();
}
// cardlist has changed
QStringList order = cards.keys();
std::sort( order.begin(), order.end(), Common::cardsOrder );
bool update = d->t.cards() != order || d->t.readers() != readers;
// check if selected card is still in slot
if( !d->t.card().isEmpty() && !order.contains( d->t.card() ) )
{
update = true;
d->t.d = new QSmartCardDataPrivate();
}
d->t.d->cards = order;
d->t.d->readers = readers;
// if none is selected select first from cardlist
if( d->t.card().isEmpty() && !d->t.cards().isEmpty() )
selectCard( d->t.cards().first() );
// read card data
if( d->t.cards().contains( d->t.card() ) && (!d->card || d->t.authCert().isNull() || d->t.signCert().isNull()) )
{
update = true;
try
{
QSmartCardDataPrivate *t = d->t.d;
std::string reader = cards.value( t->card );
delete d->card;
d->card = 0;
d->card = new EstEidCard( reader );
d->card->setReaderLanguageId( d->lang );
d->updateCounters( t );
std::vector<std::string> data;
d->card->readPersonalData( data, EstEidCard::SURNAME, EstEidCard::COMMENT4 );
ByteVec authcert = d->card->getAuthCert();
ByteVec signcert = d->card->getSignCert();
t->reader = reader.c_str();
t->pinpad = d->card->isSecureConnection();
t->version = QSmartCardData::CardVersion(d->card->getCardVersion());
try {
if( t->version > QSmartCardData::VER_1_1 )
{
d->card->sendApplicationIdentifierPreV3();
t->version = QSmartCardData::VER_3_0;
}
} catch( const std::runtime_error &e ) {
qDebug() << Q_FUNC_INFO << "Card is not V3.0: " << e.what();
}
t->data[QSmartCardData::SurName] =
SslCertificate::formatName( d->encode( data[EstEidCard::SURNAME] ) ).trimmed();
t->data[QSmartCardData::FirstName] =
SslCertificate::formatName( d->encode( data[EstEidCard::FIRSTNAME] ) ).trimmed();
t->data[QSmartCardData::MiddleName] =
SslCertificate::formatName( d->encode( data[EstEidCard::MIDDLENAME] ) ).trimmed();
t->data[QSmartCardData::Sex] = d->encode( data[EstEidCard::SEX] );
t->data[QSmartCardData::Citizen] = d->encode( data[EstEidCard::CITIZEN] );
t->data[QSmartCardData::BirthDate] = QDate::fromString( d->encode( data[EstEidCard::BIRTHDATE] ), "dd.MM.yyyy" );
t->data[QSmartCardData::Id] = d->encode( data[EstEidCard::ID] );
t->data[QSmartCardData::DocumentId] = d->encode( data[EstEidCard::DOCUMENTID] );
t->data[QSmartCardData::Expiry] = QDate::fromString( d->encode( data[EstEidCard::EXPIRY] ), "dd.MM.yyyy" );
t->data[QSmartCardData::BirthPlace] =
SslCertificate::formatName( d->encode( data[EstEidCard::BIRTHPLACE] ) );
t->data[QSmartCardData::IssueDate] = QDate::fromString( d->encode( data[EstEidCard::ISSUEDATE] ), "dd.MM.yyyy" );
t->data[QSmartCardData::ResidencePermit] = d->encode( data[EstEidCard::RESIDENCEPERMIT] );
t->data[QSmartCardData::Comment1] = d->encode( data[EstEidCard::COMMENT1] );
t->data[QSmartCardData::Comment2] = d->encode( data[EstEidCard::COMMENT2] );
t->data[QSmartCardData::Comment3] = d->encode( data[EstEidCard::COMMENT3] );
t->data[QSmartCardData::Comment4] = d->encode( data[EstEidCard::COMMENT4] );
if( !authcert.empty() )
t->authCert = QSslCertificate( QByteArray( (char*)&authcert[0], (int)authcert.size() ), QSsl::Der );
if( !signcert.empty() )
t->signCert = QSslCertificate( QByteArray( (char*)&signcert[0], (int)signcert.size() ), QSsl::Der );
QStringList mailaddresses = t->authCert.alternateSubjectNames().values( QSsl::EmailEntry );
t->data[QSmartCardData::Email] = !mailaddresses.isEmpty() ? mailaddresses.first() : "";
QStringList name = QStringList()
<< t->data[QSmartCardData::FirstName].toString()
<< t->data[QSmartCardData::MiddleName].toString()
<< t->data[QSmartCardData::SurName].toString();
name.removeAll( "" );
t->data[QSmartCardData::FullName] = name.join( " " );
if( t->authCert.type() & SslCertificate::DigiIDType )
{
t->data[QSmartCardData::FullName] = t->authCert.toString( "GN SN" );
t->data[QSmartCardData::Id] = t->authCert.subjectInfo("serialNumber");
t->data[QSmartCardData::BirthDate] = IKValidator::birthDate( t->authCert.subjectInfo("serialNumber") );
t->data[QSmartCardData::IssueDate] = t->authCert.effectiveDate();
t->data[QSmartCardData::Expiry] = t->authCert.expiryDate();
}
}
catch( const std::runtime_error &e )
{
qDebug() << Q_FUNC_INFO << "Error on loading card data: " << e.what();
}
}
// update data if something has changed
if( update )
Q_EMIT dataChanged();
d->m.unlock();
}
else if( d->e.isRunning() )
{
emit eventStarted();
try
{
byte retries = 0;
d->result = QSmartCard::UnknownError;
switch( d->cmd )
{
case QSmartCardPrivate::Change:
switch( d->type )
{
case QSmartCardData::Pin1Type:
d->card->changeAuthPin( d->pin, d->old, retries );
break;
case QSmartCardData::Pin2Type:
d->card->changeSignPin( d->pin, d->old, retries );
break;
case QSmartCardData::PukType:
d->card->changePUK( d->pin, d->old, retries );
break;
}
d->updateCounters( d->t.d );
break;
case QSmartCardPrivate::Unblock:
switch( d->type )
{
case QSmartCardData::Pin1Type:
d->card->unblockAuthPin( d->pin, d->old, retries );
break;
case QSmartCardData::Pin2Type:
d->card->unblockSignPin( d->pin, d->old, retries );
break;
default: break;
}
d->updateCounters( d->t.d );
break;
case QSmartCardPrivate::Validate:
switch( d->type )
{
case QSmartCardData::Pin1Type:
d->card->validateAuthPin( d->pin, retries );
break;
case QSmartCardData::Pin2Type:
d->card->validateSignPin( d->pin, retries );
break;
case QSmartCardData::PukType:
d->card->validatePuk( d->pin, retries );
break;
default: break;
}
break;
case QSmartCardPrivate::ValidateInternal:
switch( d->type )
{
case QSmartCardData::Pin1Type:
d->card->enterPin( EstEidCard::PIN_AUTH, d->pin );
break;
case QSmartCardData::Pin2Type:
d->card->enterPin( EstEidCard::PIN_SIGN, d->pin );
break;
case QSmartCardData::PukType:
d->card->enterPin( EstEidCard::PUK, d->pin );
break;
}
break;
default: break;
}
d->result = QSmartCard::NoError;
}
catch( const AuthError &e )
{
d->result = d->handleAuthError(
d->cmd == QSmartCardPrivate::Unblock ? QSmartCardData::PukType : d->type, e );
}
catch( const std::runtime_error &e )
{
qDebug() << Q_FUNC_INFO << e.what();
}
d->pin.clear();
d->old.clear();
d->e.quit();
}
sleep( 5 );
}
}
// Called within a transaction
ByteVec EstEidCard::RSADecrypt_internal(const ByteVec &cryptogram)
{
mManager.writeLog("[%s:%d]", __FUNCTION__, __LINE__);
if (_card_version == VER_3_0 && cryptogram.size() != 256)
throw std::runtime_error("2048 bit keys but cryptogram is not 256 bytes!");
if (_card_version != VER_3_0 && cryptogram.size() != 128)
throw std::runtime_error("cryptogram is not 128 bytes!");
byte modEnv1[] = {0x00,0x22,0x41,0xA4,0x02,0x83,0x00};
byte modEnv2[] = {0x00,0x22,0x41,0xB6,0x02,0x83,0x00};
byte modEnv0[] = {0x00,0x22,0x41,0xB8,0x05,0x83,0x03,0x80};
//mManager.beginTransaction(mConnection);
selectMF(true);
selectDF(FILEID_APP,true);
selectEF(FILEID_KEYPOINTER,true);
ByteVec keyRec = readRecord(1);
if (keyRec.size() != 0x15)
{
mManager.endTransaction(mConnection);
throw CardDataError("key ptr len is not 0x15");
}
ByteVec cmd(MAKEVECTOR(modEnv0));
cmd.insert(cmd.end(),&keyRec[0x9],&keyRec[0xB]);
setSecEnv(6);
execute(MAKEVECTOR(modEnv1));
execute(MAKEVECTOR(modEnv2));
execute(cmd);
ByteVec result;
try
{
if (_card_version == VER_3_0) {
// Use chaining for decryption
byte decrypt_chain1[] = {0x10, 0x2A, 0x80, 0x86, 0xFF, 0x00};
byte decrypt_chain2[] = {0x00, 0x2A, 0x80, 0x86, 0x02};
ByteVec first_command(MAKEVECTOR(decrypt_chain1));
ByteVec second_command(MAKEVECTOR(decrypt_chain2));
first_command.insert(first_command.end(), cryptogram.begin(), cryptogram.end()-2);
second_command.insert(second_command.end(), cryptogram.end()-2, cryptogram.end());
execute(first_command, 2);
result = execute(second_command, 4);
} else {
byte decrypt[] = {0x00, 0x2A, 0x80, 0x86, 0x81, 0x00};
ByteVec decrypt_command(MAKEVECTOR(decrypt));
decrypt_command.insert(decrypt_command.end(), cryptogram.begin(), cryptogram.end());
result = execute(decrypt_command, 4);
}
}
catch(CardError e)
{
mManager.endTransaction(mConnection);
if (e.SW1 == 0x69 && (e.SW2 == 0x82 || e.SW2 == 0x00 || e.SW2 == 0x88 || e.SW2 == 0x85
/*|| e.SW1 == 0x64 */|| e.SW1 == 0x6B ))
throw AuthError(e);
throw e;
}
mManager.endTransaction(mConnection);
return result;
}
ByteVec EstEidCard::calcSign_internal(AlgType type,KeyType keyId,const ByteVec &hash,bool withOID)
{
mManager.writeLog("[%s:%d]", __FUNCTION__, __LINE__);
byte signCmdSig[] = {0x00,0x2A,0x9E,0x9A};
byte signCmdAut[] = {0x00,0x88,0x00,0x00};
byte hashHdMD5[] = {0x30,0x20,0x30,0x0C,0x06,0x08,0x2A,0x86,0x48,0x86,0xF7,0x0D,0x02,0x05,0x05,0x00,0x04,0x10};
byte hashHdSHA1[] = {0x30,0x21,0x30,0x09,0x06,0x05,0x2B,0x0E,0x03,0x02,0x1A,0x05,0x00,0x04,0x14};
byte hashHdSHA224[] = {0x30,0x2d,0x30,0x0d,0x06,0x09,0x60,0x86,0x48,0x01,0x65,0x03,0x04,0x02,0x04,0x05,0x00,0x04,0x1c};
byte hashHdSHA256[] = {0x30,0x31,0x30,0x0d,0x06,0x09,0x60,0x86,0x48,0x01,0x65,0x03,0x04,0x02,0x01,0x05,0x00,0x04,0x20};
byte hashHdSHA384[] = {0x30,0x41,0x30,0x0d,0x06,0x09,0x60,0x86,0x48,0x01,0x65,0x03,0x04,0x02,0x02,0x05,0x00,0x04,0x30};
byte hashHdSHA512[] = {0x30,0x51,0x30,0x0d,0x06,0x09,0x60,0x86,0x48,0x01,0x65,0x03,0x04,0x02,0x03,0x05,0x00,0x04,0x40};
ByteVec cmd,header;
if (keyId == 0 )
{
cmd = MAKEVECTOR(signCmdAut);
}
else
{
cmd = MAKEVECTOR(signCmdSig);
}
if (withOID)
{
switch(type)
{
case MD5:
header = MAKEVECTOR(hashHdMD5);
break;
case SHA1:
header = MAKEVECTOR(hashHdSHA1);
break;
case SHA224:
header = MAKEVECTOR(hashHdSHA224);
break;
case SHA256:
header = MAKEVECTOR(hashHdSHA256);
break;
case SHA384:
header = MAKEVECTOR(hashHdSHA384);
break;
case SHA512:
header = MAKEVECTOR(hashHdSHA512);
break;
default:
throw std::runtime_error("cannot calculate SSL signature with OID");
}
cmd.push_back((byte) (header.size() + hash.size()));
cmd.insert(cmd.end(), header.begin(), header.end());
}
else
cmd.push_back((byte)hash.size());
setSecEnv(1);
cmd.insert(cmd.end(), hash.begin(), hash.end());
ByteVec result;
try
{
//#ifdef _WIN32
//setVersion();//SIGSEGs Under Linux. Needed only for Windows MiniDriver
//#endif
if(_card_version != VER_3_0)
{
mManager.writeLog("[%s:%d] Sending APDU case 4 (Card version %i)", __FUNCTION__, __LINE__, getCardVersion());
result = execute(cmd, 4);
}
else
{
mManager.writeLog("[%s:%d] Sending default APDU case (Card version %i)", __FUNCTION__, __LINE__, getCardVersion());
result = execute(cmd);
}
}
catch(CardError e)
{
if (e.SW1 == 0x69 && (e.SW2 == 0x82 || e.SW2 == 0x00 || e.SW2 == 0x85 ))
throw AuthError(e);
throw e;
}
return result;
}
