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); } }
ByteVec read_file(const char* path) throw(std::runtime_error) { const size_t CHUNK_SIZE = 32 * 1024; // 32kB ~= 1-2 kloc FILE* fp = fopen(path, "rb"); if (!fp) { throw std::runtime_error("File not found: " + std::string(path)); } ByteVec buff; size_t num_read = 0; while (feof(fp) == 0) { buff.resize(num_read + CHUNK_SIZE); size_t n = fread(&buff[num_read], 1, CHUNK_SIZE, fp); num_read += n; buff.resize(num_read); } fclose(fp); return buff; }
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::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; }
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; }
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; }
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]; } } } }