/**********************************************************************************************
* parseMmsPdu
* парсит MMS PDU
*********************************************************************************************/
static inline MmsIndication parseMmsPdu(MmsServerConnection* self, ByteBuffer* message, ByteBuffer* response)
{
MmsPdu_t* mmsPdu = 0; /* allow asn1c to allocate structure */
MmsIndication retVal;
asn_dec_rval_t rval; /* Decoder return value */
rval = ber_decode(NULL, &asn_DEF_MmsPdu, (void**) &mmsPdu, (void*) message->buffer, message->maxSize/*size*/);
if (DEBUG) xer_fprint(stdout, &asn_DEF_MmsPdu, mmsPdu);
if (rval.code != RC_OK) {
retVal = MMS_ERROR;
goto parseMmsPdu_exit;
}
switch (mmsPdu->present) {
case MmsPdu_PR_initiateRequestPdu:
handleInitiateRequestPdu(self, &(mmsPdu->choice.initiateRequestPdu), response);
retVal = MMS_INITIATE;
break;
case MmsPdu_PR_confirmedRequestPdu:
handleConfirmedRequestPdu(self, &(mmsPdu->choice.confirmedRequestPdu), response);
retVal = MMS_CONFIRMED_REQUEST;
break;
case MmsPdu_PR_concludeRequestPDU:
mmsServer_writeConcludeResponsePdu(response);
IsoConnection_close(self->isoConnection);
retVal = MMS_CONCLUDE;
break;
default:
writeMmsRejectPdu(NULL, REJECT_UNKNOWN_PDU_TYPE, response);
retVal = MMS_ERROR;
break;
}
parseMmsPdu_exit:
asn_DEF_MmsPdu.free_struct(&asn_DEF_MmsPdu, mmsPdu, 0);
return retVal;
}
static void
check_1(int is_ok, uint8_t *buf, size_t size, size_t consumed) {
asn_TYPE_descriptor_t *td = &asn_DEF_T1;
asn_dec_rval_t rval;
T1_t t, *tp;
tp = memset(&t, 0, sizeof(t));
fprintf(stderr, "Buf %p\n", buf);
rval = ber_decode(0, td, (void **)&tp, buf, size);
fprintf(stderr, "Returned code %d, consumed %zd\n",
(int)rval.code, rval.consumed);
if(is_ok) {
assert(rval.code == RC_OK);
assert(rval.consumed == consumed);
} else {
if(rval.code == RC_OK) {
}
assert(rval.consumed <= consumed);
}
}
int main(int ac, char **av) {
char buf[1024];
asn_dec_rval_t rval;
Rectangle_t *rectangle = 0;
FILE *fp;
size_t size;
char *filename;
if(ac != 2) {
fprintf(stderr, "Usage: %s <file.ber>\n", av[0]);
exit(1);
} else {
filename = av[1];
}
fp = fopen(filename, "rb");
if(!fp) {
perror(filename);
exit(1);
}
size = fread(buf, 1, sizeof(buf), fp);
fclose(fp);
if(!size) {
fprintf(stderr, "%s: Empty or broken\n", filename);
exit(1);
}
rval = ber_decode(0, &asn_DEF_Rectangle, (void **)&rectangle, buf, size);
if(rval.code != RC_OK) {
fprintf(stderr, "%s: Broken Rectangle encoding at byte %ld\n", filename,
(long)rval.consumed);
exit(1);
}
xer_fprint(stdout, &asn_DEF_Rectangle, rectangle);
return 0;
}
void
mmsServer_handleDefineNamedVariableListRequest(
MmsServerConnection connection,
uint8_t* buffer, int bufPos, int maxBufPos,
uint32_t invokeId,
ByteBuffer* response)
{
DefineNamedVariableListRequest_t* request = 0;
MmsPdu_t* mmsPdu = 0;
asn_dec_rval_t rval = ber_decode(NULL, &asn_DEF_MmsPdu, (void**) &mmsPdu, buffer, maxBufPos);
if (rval.code != RC_OK) {
mmsServer_writeMmsRejectPdu(&invokeId, MMS_ERROR_REJECT_INVALID_PDU, response);
goto exit_function;
}
request = &(mmsPdu->choice.confirmedRequestPdu.confirmedServiceRequest.choice.defineNamedVariableList);
MmsDevice* device = MmsServer_getDevice(connection->server);
if (request->variableListName.present == ObjectName_PR_domainspecific) {
char domainName[65];
if (request->variableListName.choice.domainspecific.domainId.size > 64) {
mmsServer_createConfirmedErrorPdu(invokeId, response, MMS_ERROR_ACCESS_OBJECT_NON_EXISTENT);
goto exit_free_struct;
}
StringUtils_createStringFromBufferInBuffer(domainName,
request->variableListName.choice.domainspecific.domainId.buf,
request->variableListName.choice.domainspecific.domainId.size);
MmsDomain* domain = MmsDevice_getDomain(device, domainName);
if (domain == NULL) {
mmsServer_createConfirmedErrorPdu(invokeId, response, MMS_ERROR_ACCESS_OBJECT_NON_EXISTENT);
goto exit_free_struct;
}
if (LinkedList_size(domain->namedVariableLists) < CONFIG_MMS_MAX_NUMBER_OF_DOMAIN_SPECIFIC_DATA_SETS) {
char variableListName[65];
if (request->variableListName.choice.domainspecific.itemId.size > 64) {
mmsServer_createConfirmedErrorPdu(invokeId, response, MMS_ERROR_ACCESS_OBJECT_NON_EXISTENT);
goto exit_free_struct;
}
StringUtils_createStringFromBufferInBuffer(variableListName,
request->variableListName.choice.domainspecific.itemId.buf,
request->variableListName.choice.domainspecific.itemId.size);
if (MmsDomain_getNamedVariableList(domain, variableListName) != NULL) {
mmsServer_createConfirmedErrorPdu(invokeId, response, MMS_ERROR_DEFINITION_OBJECT_EXISTS);
}
else {
MmsError mmsError;
MmsNamedVariableList namedVariableList = createNamedVariableList(domain, device,
request, variableListName, &mmsError);
if (namedVariableList != NULL) {
mmsError = mmsServer_callVariableListChangedHandler(true, MMS_DOMAIN_SPECIFIC, domain, variableListName, connection);
if (mmsError == MMS_ERROR_NONE) {
MmsDomain_addNamedVariableList(domain, namedVariableList);
createDefineNamedVariableListResponse(invokeId, response);
}
else {
MmsNamedVariableList_destroy(namedVariableList);
mmsServer_createConfirmedErrorPdu(invokeId, response, mmsError);
}
}
else
mmsServer_createConfirmedErrorPdu(invokeId, response, mmsError);
}
}
else
mmsServer_createConfirmedErrorPdu(invokeId, response, MMS_ERROR_RESOURCE_CAPABILITY_UNAVAILABLE);
}
else if (request->variableListName.present == ObjectName_PR_aaspecific) {
if (LinkedList_size(connection->namedVariableLists) < CONFIG_MMS_MAX_NUMBER_OF_ASSOCIATION_SPECIFIC_DATA_SETS) {
char variableListName[65];
if (request->variableListName.choice.aaspecific.size > 64) {
mmsServer_createConfirmedErrorPdu(invokeId, response, MMS_ERROR_ACCESS_OBJECT_NON_EXISTENT);
goto exit_free_struct;
}
StringUtils_createStringFromBufferInBuffer(variableListName,
request->variableListName.choice.aaspecific.buf,
request->variableListName.choice.aaspecific.size);
if (MmsServerConnection_getNamedVariableList(connection, variableListName) != NULL) {
mmsServer_createConfirmedErrorPdu(invokeId, response, MMS_ERROR_DEFINITION_OBJECT_EXISTS);
}
else {
MmsError mmsError;
MmsNamedVariableList namedVariableList = createNamedVariableList(NULL, device,
request, variableListName, &mmsError);
if (namedVariableList != NULL) {
if (mmsServer_callVariableListChangedHandler(true, MMS_ASSOCIATION_SPECIFIC, NULL, variableListName, connection) == MMS_ERROR_NONE) {
MmsServerConnection_addNamedVariableList(connection, namedVariableList);
createDefineNamedVariableListResponse(invokeId, response);
}
else {
MmsNamedVariableList_destroy(namedVariableList);
mmsServer_createConfirmedErrorPdu(invokeId, response, MMS_ERROR_ACCESS_OBJECT_ACCESS_DENIED);
}
}
else
mmsServer_createConfirmedErrorPdu(invokeId, response, mmsError);
}
}
else
mmsServer_createConfirmedErrorPdu(invokeId, response, MMS_ERROR_RESOURCE_CAPABILITY_UNAVAILABLE);
}
else
mmsServer_createConfirmedErrorPdu(invokeId, response, MMS_ERROR_DEFINITION_TYPE_UNSUPPORTED);
exit_free_struct:
asn_DEF_MmsPdu.free_struct(&asn_DEF_MmsPdu, mmsPdu, 0);
exit_function:
return;
}
void
mmsServer_handleDeleteNamedVariableListRequest(MmsServerConnection connection,
uint8_t* buffer, int bufPos, int maxBufPos,
uint32_t invokeId,
ByteBuffer* response)
{
DeleteNamedVariableListRequest_t* request = 0;
MmsPdu_t* mmsPdu = 0;
asn_dec_rval_t rval = ber_decode(NULL, &asn_DEF_MmsPdu, (void**) &mmsPdu, buffer, maxBufPos);
if (rval.code != RC_OK) {
mmsServer_writeMmsRejectPdu(&invokeId, MMS_ERROR_REJECT_INVALID_PDU, response);
goto exit_function;
}
request = &(mmsPdu->choice.confirmedRequestPdu.confirmedServiceRequest.choice.deleteNamedVariableList);
long scopeOfDelete;
asn_INTEGER2long(request->scopeOfDelete, &scopeOfDelete);
MmsDevice* device = MmsServer_getDevice(connection->server);
if (scopeOfDelete == DeleteNamedVariableListRequest__scopeOfDelete_specific) {
int numberMatched = 0;
int numberDeleted = 0;
int numberItems = request->listOfVariableListName->list.count;
int i;
for (i = 0; i < numberItems; i++) {
if (request->listOfVariableListName->list.array[i]->present == ObjectName_PR_domainspecific) {
char domainName[65];
char listName[65];
mmsMsg_copyAsn1IdentifierToStringBuffer(request->listOfVariableListName->list.array[i]->choice.domainspecific.domainId,
domainName, 65);
mmsMsg_copyAsn1IdentifierToStringBuffer(request->listOfVariableListName->list.array[i]->choice.domainspecific.itemId,
listName, 65);
MmsDomain* domain = MmsDevice_getDomain(device, domainName);
if (domain != NULL) {
MmsNamedVariableList variableList = MmsDomain_getNamedVariableList(domain, listName);
if (variableList != NULL) {
numberMatched++;
if (MmsNamedVariableList_isDeletable(variableList)) {
if (mmsServer_callVariableListChangedHandler(false, MMS_DOMAIN_SPECIFIC, domain, listName, connection) == MMS_ERROR_NONE) {
MmsDomain_deleteNamedVariableList(domain, listName);
numberDeleted++;
}
}
}
}
}
else if (request->listOfVariableListName->list.array[i]->present == ObjectName_PR_aaspecific) {
char listName[65];
mmsMsg_copyAsn1IdentifierToStringBuffer(request->listOfVariableListName->list.array[i]->choice.aaspecific,
listName, 65);
MmsNamedVariableList variableList = MmsServerConnection_getNamedVariableList(connection, listName);
if (variableList != NULL) {
numberMatched++;
if (mmsServer_callVariableListChangedHandler(false, MMS_ASSOCIATION_SPECIFIC, NULL, listName, connection) == MMS_ERROR_NONE) {
numberDeleted++;
MmsServerConnection_deleteNamedVariableList(connection, listName);
}
}
}
}
createDeleteNamedVariableListResponse(invokeId, response, numberMatched, numberDeleted);
}
else {
mmsServer_createConfirmedErrorPdu(invokeId, response, MMS_ERROR_ACCESS_OBJECT_ACCESS_UNSUPPORTED);
}
asn_DEF_MmsPdu.free_struct(&asn_DEF_MmsPdu, mmsPdu, 0);
exit_function:
return;
}
/*
WSZ porzdki - BMD_ERR_LOAD_SIG_POLICY (-77)
@return BMD_ERR_LOAD_SIG_POLICY (-77) jesli nie mona zaladowac badz zdekodowac wskazanej polityki podpisu
@return BMDPKI_ERR_FIND_CERT_STORE (-78) jesli nie mozna odnalezc magazynu certyfikatow zaufanych
z funkcji bmd_verify_cert_with_crl():
@return BMD_ERR_PARAM1 - niewlasciwe wywolanie
@return BMD_ERR_OP_FAILED - wiele bledow nie okreslonych blizej
@return BMD_ERR_PKI_NO_CRL (-56) - nie moze wczytac listy CRL
@return BMD_ERR_PKI_VERIFY - blad weryfikacji certyfikatu
*/
long bmd_set_signature_params_policy(bmd_signature_params_t *sig_params,char *sig_policy,long auto_params_create,GenBuf_t *cert, CertExtConfig_t *certCheckParams)
{
long status;
GenBuf_t *buf=NULL;
SignaturePolicy_t *policy=NULL;
asn_dec_rval_t rval;
CMSAttrs_t *policy_signed_attrs=NULL;
SignerAndVerifierRules_t *SigVerRules=NULL;
long i=0;
long j=0;
long type;
char *policy_oid=NULL;
bmd_crypt_ctx_t *hash_ctx=NULL;
GenBuf_t *policy_hash_buf=NULL;
GenBuf_t *cert_hash_buf=NULL;
Attribute_t *sig_pol_attr=NULL;
Attribute_t *cert_attr=NULL;
Attribute_t *time_attr=NULL;
Certificate_t *asn1_cert=NULL;
CertificateTrustPoint_t *trust_point=NULL;
CertificateTrustTrees_t *trust_trees=NULL;
SigningCertTrustCondition_t *sig_cert_trust=NULL;
GeneralNames_t *g_names=NULL;
GeneralName_t *g_name=NULL;
char *policy_url=NULL;
long trust_condition=0;
GenBuf_t **trusted_certs=NULL;
GenBuf_t **constructed_cert_path=NULL;
long cert_path_length=0;
long flag_trust_found=0;
if(sig_params==NULL)
{
PRINT_DEBUG("LIBBMDPKIERR Invalid first parameter value. Error=%i\n",BMD_ERR_PARAM1);
return BMD_ERR_PARAM1;
}
if(sig_policy==NULL)
{
PRINT_DEBUG("LIBBMDPKIERR Invalid second parameter value. Error=%i\n",BMD_ERR_PARAM2);
return BMD_ERR_PARAM2;
}
status=_bmd_load_signature_policy(sig_policy,&buf);
if(status!=BMD_OK)
{
PRINT_DEBUG("LIBBMDPKIERR Error loading signature policy. Error=%i\n",BMDPKI_ERR_LOAD_SIG_POLICY);
return BMDPKI_ERR_LOAD_SIG_POLICY;
}
rval=ber_decode(NULL,&asn_DEF_SignaturePolicy,&(sig_params->signature_policy),buf->buf,buf->size);
if(rval.code!=RC_OK)
{
PRINT_DEBUG("LIBBMDPKIERR Error loading signature policy. Error=%i\n",BMDPKI_ERR_LOAD_SIG_POLICY);
return BMDPKI_ERR_LOAD_SIG_POLICY;
}
policy=(SignaturePolicy_t *)sig_params->signature_policy;
/* pobranie zaufanych certyfikatow dla podpisujacego z polityki podpisu */
trust_trees=&(policy->signPolicyInfo.signatureValidationPolicy.commonRules.signingCertTrustCondition->signerTrustTrees);
trusted_certs=(GenBuf_t **)calloc((trust_trees->list.count+2), sizeof(GenBuf_t *));
for(i=0;i<trust_trees->list.count;i++)
{
trust_point=trust_trees->list.array[i];
asn1_encode(&asn_DEF_Certificate,&(trust_point->trustpoint), NULL, &(trusted_certs[i]));
}
if(sig_params->tcc) /* jesli ustawiono Trusted Cert Store */
{
status=bmd_verify_cert_path(cert,sig_params->tcc,&constructed_cert_path,NULL);
if(status==BMD_OK)
{
i=0;
while(constructed_cert_path[i])
{
i++;
}
cert_path_length=i;
i=0;
while(trusted_certs[i])
{
if(trusted_certs[i]->size==constructed_cert_path[cert_path_length-1]->size)
{
if(memcmp(trusted_certs[i]->buf,constructed_cert_path[cert_path_length-1]->buf,trusted_certs[i]->size)==0)
{
flag_trust_found=1;
break;
}
}
i++;
}
if(flag_trust_found==0)
{
PRINT_DEBUG("LIBBMDPKIERR Error. Error=%i\n",BMD_ERR_OP_FAILED);
return BMD_ERR_OP_FAILED;
}
}
else
{
PRINT_DEBUG("LIBBMDPKIERR Error in finding cert store. Error=%i\n",BMDPKI_ERR_FIND_CERT_STORE);
return BMDPKI_ERR_FIND_CERT_STORE;
}
}
else
{
PRINT_DEBUG("LIBBMDPKIERR Error. Error=%i\n",BMD_ERR_OP_FAILED);
return BMD_ERR_OP_FAILED;
}
/* sprawdzenie czy certyfikat ktorym ma sie wykonac podpis jest na CRL - jesli tylko to jest w polityce */
sig_cert_trust=policy->signPolicyInfo.signatureValidationPolicy.commonRules.signingCertTrustCondition;
asn_INTEGER2long(&(sig_cert_trust->signerRevReq.endCertRevReq.enuRevReq),&trust_condition);
if(trust_condition==EnuRevReq_clrCheck)
{
if(certCheckParams==NULL) /*na podstawie CRL distribution point w certyfikacie*/
{
status=bmd_verify_cert_with_crl(cert, NULL, NULL);
}
else if(certCheckParams->check_with_crl > 0)
{
if(certCheckParams->download_crl > 0)
{
status=bmd_verify_cert_with_crl(cert, certCheckParams->url_of_crl, NULL);
}
else if(certCheckParams->use_crl_file > 0)
{
status=bmd_verify_cert_with_crl(cert, NULL, certCheckParams->crl_file_path);
}
else /*na podstawie CRL distribution point w certyfikacie*/
{
status=bmd_verify_cert_with_crl(cert, NULL, NULL);
}
}
else /*jesli wylaczona weryfikacja crl*/
{
status=BMD_OK;
}
if(status!=BMD_OK)
{
PRINT_DEBUG("LIBBMDPKIERR Error. Error=%li\n",status);
return status;
}
}
/* w tym momencie wiemy ze certyfikat moze wykonac podpis ??? */
if(auto_params_create==1) /* stworzenie parametrow stalych do podpisu */
{
policy=(SignaturePolicy_t *)sig_params->signature_policy;
/* ustaw wskaznik na liste obowiazkowych atrybutow podpisanych */
SigVerRules=policy->signPolicyInfo.signatureValidationPolicy.commonRules.signerAndVeriferRules;
policy_signed_attrs=&(SigVerRules->signerRules.mandatedSignedAttr);
sig_params->signed_attributes=(SignedAttributes_t *)malloc(sizeof(SignedAttributes_t));
memset(sig_params->signed_attributes,0,sizeof(SignedAttributes_t));
for(i=0;i<policy_signed_attrs->list.count;i++)
{
__bmd_determine_signed_attr_type(policy_signed_attrs->list.array[i],&type);
switch(type)
{
case BMD_CMS_ATTR_CONTENT_TYPE: /* rozszerzenie standardowe - nic nie trzeba robic */
break;
case BMD_CMS_ATTR_MESSAGE_DIGEST: /* rozszerzenie standardowe - nic nie trzeba robic */
break;
case BMD_CMS_ATTR_SIGNING_TIME: /* zostanie stworzony pozniej */
SigAttr_Create_SigningTime(&time_attr);
asn_set_add(sig_params->signed_attributes,time_attr);
break;
case BMD_CMS_ATTR_SIGNING_CERT:
if(cert)
{
bmd_ctx_destroy(&hash_ctx);
bmd_set_ctx_hash(&hash_ctx,BMD_HASH_ALGO_SHA1);
status=bmd_hash_data(cert,&hash_ctx,&cert_hash_buf,NULL);
ber_decode(NULL,&asn_DEF_Certificate,(void **)&asn1_cert,cert->buf,cert->size);
SigAttr_Create_SigningCertificate(cert_hash_buf,asn1_cert,&cert_attr);
asn_set_add(sig_params->signed_attributes,cert_attr);
bmd_ctx_destroy(&hash_ctx);
asn_DEF_Certificate.free_struct(&asn_DEF_Certificate,asn1_cert,0);asn1_cert=NULL;
free_gen_buf(&cert_hash_buf);
}
else
{
PRINT_DEBUG("LIBBMDPKIERR Error. Error=%i\n",BMD_ERR_OP_FAILED);
return BMD_ERR_OP_FAILED;
}
break;
case BMD_CMS_ATTR_SIG_POLICY_ID:
status=OBJECT_IDENTIFIER2string(&(policy->signPolicyInfo.signPolicyIdentifier),&policy_oid);
if(status!=BMD_OK)
{
PRINT_DEBUG("LIBBMDPKIERR Error. Error=%i\n",BMD_ERR_OP_FAILED);
return BMD_ERR_OP_FAILED;
}
/* (by WSZ) zmiana sposobu wyliczania skrotu polityki podipsu (ProCertum API compatible)
bmd_ctx_destroy(&hash_ctx);
status=bmd_set_ctx_hash(&hash_ctx,BMD_HASH_ALGO_SHA1);
status=bmd_hash_data(buf,&hash_ctx,&policy_hash_buf,NULL);
*/
status=bmd_hash_SignaturePolicy(buf, &policy_hash_buf);
g_names=&(policy->signPolicyInfo.policyIssuerName);
for(j=0;j<g_names->list.count;j++)
{
g_name=g_names->list.array[j];
if(g_name->present==GeneralName_PR_dNSName)
{
GeneralName2string(g_name,&policy_url);
break;
}
}
SigAttr_Create_SignaturePolicyId(policy_oid,policy_url,policy_hash_buf,&sig_pol_attr);
asn_set_add(sig_params->signed_attributes,(void *)sig_pol_attr);
free(policy_oid);
free(policy_url);
bmd_ctx_destroy(&hash_ctx);
free_gen_buf(&policy_hash_buf);
break;
}
}
}
i=0;
if(trusted_certs)
{
while(trusted_certs[i])
{
free_gen_buf(&(trusted_certs[i]));
i++;
}
free(trusted_certs);trusted_certs=NULL;
}
i=0;
if(constructed_cert_path)
{
while(constructed_cert_path[i])
{
free_gen_buf(&(constructed_cert_path[i]));
i++;
}
free(constructed_cert_path);constructed_cert_path=NULL;
}
return BMD_OK;
}
int main() {
initialise_termination_handler();
cv::namedWindow("input",1);
int announce_socket;
in_addr iaddr;
memset(&iaddr, 0, sizeof(struct in_addr));
announce_socket = socket(AF_INET, SOCK_DGRAM, 0);
if ( announce_socket < 0 ) {
perror("Error creating socket");
exit(0);
}
int one = 1;
int ret = setsockopt(announce_socket, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one));
if (ret < 0) {
perror("setsockopt");
return 1;
}
sockaddr_in saddr;
memset(&saddr, 0, sizeof(struct sockaddr_in));
saddr.sin_family = AF_INET;
saddr.sin_port = htons(CAMERA_ANNOUNCE_PORT);
// saddr.sin_port = 0; // Use the first free port
saddr.sin_addr.s_addr = INADDR_ANY; // bind socket to any interface
int status = bind(announce_socket, (struct sockaddr *)&saddr, sizeof(sockaddr_in));
if ( status < 0 )
perror("Error binding socket to interface"), exit(0);
ip_mreq mreq;
memset(&mreq,0,sizeof(mreq));
mreq.imr_multiaddr.s_addr=inet_addr(CAMERA_ANNOUNCE_GROUP);
mreq.imr_interface.s_addr=INADDR_ANY;
if (setsockopt(announce_socket,IPPROTO_IP,IP_ADD_MEMBERSHIP,&mreq,sizeof(mreq)) < 0) {
perror("setsockopt");
exit(1);
}
while (running == 1) {
BufferReference readyBuffer;
memset(&readyBuffer,0,sizeof(readyBuffer));
msghdr socket_message;
memset(&socket_message,0,sizeof(socket_message));
int result;
std::vector<char> data(1024);
result = recvfrom(announce_socket,data.data(),data.size(),0,NULL,NULL);
if (result <= 0) {
perror("recvfrom");
break;
}
void* input_pointer = &readyBuffer;
ber_decode(0,&asn_DEF_BufferReference,&input_pointer,data.data(),result);
std::string buffer_name = "/" + get_name_of_buffer(readyBuffer.sequence);
int buffer_descriptor = shm_open(buffer_name.c_str(), O_RDONLY, 0);
if (buffer_descriptor == -1) {
std::cout << "Can not open shared buffer file " << buffer_name << std::endl;
continue;
}
size_t buffer_length = readyBuffer.width*readyBuffer.height*2;
void* pointer = mmap(NULL,buffer_length, PROT_READ,MAP_PRIVATE,buffer_descriptor,0);
close(buffer_descriptor);
if (pointer == MAP_FAILED) {
std::cout << "Can not map shared buffer " << buffer_name << std::endl;
continue;
}
yuy2::view_t frame = boost::gil::interleaved_view(readyBuffer.width,readyBuffer.height,static_cast<yuy2::ptr_t>(pointer),readyBuffer.width*2);
cv::Mat input(readyBuffer.height,readyBuffer.width,CV_8UC2,(uint8_t*)pointer);
cv::Mat output(readyBuffer.height,readyBuffer.width,CV_8UC3);
cv::cvtColor(input,output,CV_YUV2BGR_YUY2);
munmap(pointer,buffer_length);
cv::imshow("input", output);
timespec tp;
clock_gettime(CLOCK_MONOTONIC,&tp);
timeval frame_timestamp{readyBuffer.timestamp_seconds, readyBuffer.timestamp_microseconds};
std::cout << "Visual delay = " << get_milliseconds_delta(frame_timestamp,tp) << " ms" << std::endl;
if(int key = cv::waitKey(1) >= 0) {
break;
}
}
close(announce_socket);
return 0;
}
int LLVMFuzzerTestOneInput(const uint8_t *Data, size_t Size) {
LogLine_t *lp = 0;
(void)ber_decode(0, &asn_DEF_LogLine, (void **)&lp, Data, Size);
ASN_STRUCT_FREE(asn_DEF_LogLine, lp);
return 0;
}
MmsValue*
mmsClient_parseReadResponse(ByteBuffer* message, uint32_t* invokeId)
{
MmsPdu_t* mmsPdu = 0; /* allow asn1c to allocate structure */
MmsIndication retVal = MMS_OK;
MmsValue* valueList = NULL;
MmsValue* value = NULL;
asn_dec_rval_t rval;
rval = ber_decode(NULL, &asn_DEF_MmsPdu,
(void**) &mmsPdu, ByteBuffer_getBuffer(message), ByteBuffer_getSize(message));
if (DEBUG) xer_fprint(stdout, &asn_DEF_MmsPdu, mmsPdu);
if (mmsPdu->present == MmsPdu_PR_confirmedResponsePdu) {
*invokeId = mmsClient_getInvokeId(&mmsPdu->choice.confirmedResponsePdu);
if (mmsPdu->choice.confirmedResponsePdu.confirmedServiceResponse.present == ConfirmedServiceResponse_PR_read) {
ReadResponse_t* response = &(mmsPdu->choice.confirmedResponsePdu.confirmedServiceResponse.choice.read);
int elementCount = response->listOfAccessResult.list.count;
if (elementCount > 1)
valueList = MmsValue_createEmtpyArray(elementCount);
int i = 0;
for (i = 0; i < elementCount; i++) {
value = NULL;
AccessResult_PR presentType = response->listOfAccessResult.list.array[i]->present;
if (presentType == AccessResult_PR_failure) {
if (DEBUG) printf("access error!\n");
retVal = MMS_ERROR;
}
else if (presentType == AccessResult_PR_array) {
value = calloc(1, sizeof(MmsValue));
value->type = MMS_ARRAY;
int elementCount =
response->listOfAccessResult.list.array[i]->choice.array.list.count;
value->value.array.size = elementCount;
value->value.array.components = calloc(elementCount, sizeof(MmsValue*));
int j;
for (j = 0; j < elementCount; j++) {
value->value.array.components[j] = mmsMsg_parseDataElement(
response->listOfAccessResult.list.array[i]->choice.array.list.array[j]);
}
}
else if (presentType == AccessResult_PR_structure) {
value = calloc(1, sizeof(MmsValue));
value->type = MMS_STRUCTURE;
int componentCount =
response->listOfAccessResult.list.array[i]->choice.structure.list.count;
value->value.structure.componentCount = componentCount;
value->value.structure.components = calloc(componentCount, sizeof(MmsValue*));
int j;
for (j = 0; j < componentCount; j++) {
value->value.structure.components[j] = mmsMsg_parseDataElement(
response->listOfAccessResult.list.array[i]->choice.structure.list.array[j]);
}
}
else if (presentType == AccessResult_PR_bitstring) {
value = calloc(1, sizeof(MmsValue));
value->type = MMS_BIT_STRING;
int size = response->listOfAccessResult.list.array[i]->choice.bitstring.size;
value->value.bitString.size = (size * 8)
- response->listOfAccessResult.list.array[i]->choice.bitstring.bits_unused;
value->value.bitString.buf = malloc(size);
memcpy(value->value.bitString.buf,
response->listOfAccessResult.list.array[i]->choice.bitstring.buf, size);
}
else if (presentType == AccessResult_PR_integer) {
long integerValue;
asn_INTEGER2long(&response->listOfAccessResult.list.array[i]->choice.integer, &integerValue);
value = MmsValue_newIntegerFromInt32((int32_t) integerValue);
}
else if (presentType == AccessResult_PR_floatingpoint) {
int size = response->listOfAccessResult.list.array[i]->choice.floatingpoint.size;
value = calloc(1, sizeof(MmsValue));
value->type = MMS_FLOAT;
if (size == 5) { /* FLOAT32 */
value->value.floatingPoint.formatWidth = 32;
value->value.floatingPoint.exponentWidth = response->listOfAccessResult.list.array[i]->choice.floatingpoint.buf[0];
uint8_t* floatBuf = (response->listOfAccessResult.list.array[i]->choice.floatingpoint.buf + 1);
value->value.floatingPoint.buf = malloc(4);
#ifdef ORDER_LITTLE_ENDIAN
memcpyReverseByteOrder(value->value.floatingPoint.buf, floatBuf, 4);
#else
memcpy(value->value.floatingPoint.buf, floatBuf, 4);
#endif
}
if (size == 9) { /* FLOAT64 */
value->value.floatingPoint.formatWidth = 64;
value->value.floatingPoint.exponentWidth = response->listOfAccessResult.list.array[i]->choice.floatingpoint.buf[0];
uint8_t* floatBuf = (response->listOfAccessResult.list.array[i]->choice.floatingpoint.buf + 1);
value->value.floatingPoint.buf = malloc(8);
#ifdef ORDER_LITTLE_ENDIAN
memcpyReverseByteOrder(value->value.floatingPoint.buf, floatBuf, 8);
#else
memcpy(value->value.floatingPoint.buf, floatBuf, 8);
#endif
}
}
else if (presentType == AccessResult_PR_utctime) {
value->type = MMS_UTC_TIME;
memcpy(value->value.utcTime,
response->listOfAccessResult.list.array[i]->choice.utctime.buf, 8);
}
else {
printf("unknown type\n");
retVal = MMS_ERROR;
}
if (elementCount > 1)
MmsValue_setArrayElement(valueList, i, value);
}
}
else {
retVal = MMS_ERROR;
}
}
else {
retVal = MMS_ERROR;
}
asn_DEF_MmsPdu.free_struct(&asn_DEF_MmsPdu, mmsPdu, 0);
if (valueList == NULL)
valueList = value;
return valueList;
}
long decode_DVCSResponse( GenBuf_t *resp_buf,
DVCSResponse_t **resp)
{
ContentInfo_t *content_info = NULL;
SignedData_t *signed_data = NULL;
DVCSResponse_t *dvcs_resp = NULL;
asn_dec_rval_t dec_result;
char oid_DVCSresp_tab[] = { DVCS_RESPONSE_OID };
char oid_SignedData_tab[] = { OID_CMS_ID_SIGNED_DATA };
long ret_val=0;
if(resp_buf == NULL) { BMD_FOK(BMD_ERR_PARAM1); }
if(resp_buf->buf == NULL) { BMD_FOK(BMD_ERR_PARAM1); }
if(resp == NULL) { BMD_FOK(BMD_ERR_PARAM2); }
if(*resp != NULL) { BMD_FOK(BMD_ERR_PARAM2); }
//dekodowanie bufora do ContentInfo
dec_result=ber_decode(0, &asn_DEF_ContentInfo, (void**)&content_info, resp_buf->buf, resp_buf->size);
if(dec_result.code != RC_OK) { BMD_FOK(-5); }
//sprawdzanie,czy faktycznie SignedData
ret_val=cmp_OID_with_charTab( &(content_info->contentType), oid_SignedData_tab);
if(ret_val != 0)
{
asn_DEF_ContentInfo.free_struct(&asn_DEF_ContentInfo, content_info, 1);
free(content_info);
BMD_FOK(-6);
}
//dekodowanie zawartosci ContentInfo do SignedData
dec_result=ber_decode(0, &asn_DEF_SignedData, (void**)&signed_data, content_info->content.buf, content_info->content.size);
if(dec_result.code != RC_OK)
{
asn_DEF_ContentInfo.free_struct(&asn_DEF_ContentInfo, content_info, 1);
free(content_info);
BMD_FOK(-7);
}
//ContentInfo juz nie potrzebny
asn_DEF_ContentInfo.free_struct(&asn_DEF_ContentInfo, content_info, 1);
free(content_info);
//sprawdzanie, czy w SignedData znajduje sie DVCSResponse
ret_val=cmp_OID_with_charTab( &(signed_data->encapContentInfo.eContentType), oid_DVCSresp_tab); //libbmdpki
if(ret_val != 0)
{
asn_DEF_SignedData.free_struct(&asn_DEF_SignedData, signed_data, 1);
free(signed_data);
BMD_FOK(-8);
}
//decodowanie zawartosci SignedData do DVCSResponse
dec_result=ber_decode(0, &asn_DEF_DVCSResponse, (void**)&dvcs_resp, signed_data->encapContentInfo.eContent->buf, signed_data->encapContentInfo.eContent->size);
//SignedData nie jest juz potrzebne
asn_DEF_SignedData.free_struct(&asn_DEF_SignedData, signed_data, 1);
free(signed_data);
if(dec_result.code != RC_OK) { BMD_FOK(-9); }
*resp=dvcs_resp;
return 0;
}
u8
Decode_DER_Packet(u8 * buf, MyPacket * Pkt)
{
u8 *oid;
u8 oid_size;
/*
* MyPacket_t and MyInt_t declared in MyPacket.h and MyInt.h
*/
MyPacket_t *myPacket = 0;
MyInt_t *myInt;
asn_dec_rval_t er; /* decoder return value */
u8 ret, i;
/*
* Decode the buffer into the myPacket structure
* The asn1c compiler provides the generic BER decoder which
* is implicitly capable of decoding BER, CER and DER encoded data.
*/
er = ber_decode(0, &asn_DEF_MyPacket, (void **) &myPacket, buf,
MAXPKTSIZE);
if (er.code != RC_OK)
goto fail;
else
{
printf("Structure successfully decoded\n");
/*xer_fprint(stdout, &asn_DEF_MyPacket, myPacket);*/
}
/*
* Extract the OBJECT_IDENTIFIER into oid / oid_size
*/
oid_size = OBJECT_IDENTIFIER_get_arcs(&myPacket->myObjectId, 0,
sizeof(oid[0]), 0);
oid = malloc(oid_size * sizeof(oid[0]));
if (oid) {
(void) OBJECT_IDENTIFIER_get_arcs(&myPacket->myObjectId, oid,
sizeof(oid[0]), oid_size);
free (oid);
oid = NULL;
}
/*
* Extract MyInts
*/
for (i = 0; i < myPacket->myInts.list.count; i++)
Pkt->myInts[i] = *myPacket->myInts.list.array[i];
/*
* Extract MyFlags
*/
Pkt->myFlags = 0;
for (i = 0; i < 8; i++)
if (myPacket->myFlags.buf[0] & (1 << i))
Pkt->myFlags |= (1 << (7 - i));
/*
* Extract myName
*/
Pkt->myName = calloc(1, myPacket->myName.size + 1);
strcpy(Pkt->myName, myPacket->myName.buf);
asn_DEF_MyPacket.free_struct (&asn_DEF_MyPacket, myPacket, 0);
return er.consumed;
fail:
fprintf(stderr, "Decode failure %s\n", strerror(errno));
asn_DEF_MyPacket.free_struct(&asn_DEF_MyPacket, myPacket, 0);
return -1;
}
/*dobieranie sie do OIDow atrybutow podpisanych i niepodpisanych w strukturze podpisu*/
int main()
{
asn_dec_rval_t dec_result;
GenBuf_t *buff=NULL;
load_binary_content("test.txt..zew.cck002.sig", &buff);
SignedData_t *struktura_podpisu=NULL;
ContentInfo_t *podpis=NULL;
podpis=(ContentInfo_t*)calloc(1, sizeof(ContentInfo_t));
dec_result=ber_decode(0, &asn_DEF_ContentInfo, (void**)&podpis, buff->buf, buff->size);
free(buff->buf);
if(dec_result.code != RC_OK)
{
asn_DEF_ContentInfo.free_struct(&asn_DEF_ContentInfo, podpis, 1);
free(podpis);
printf("nie udalo sie\n");
return -1;
}
/*printf("udalo sie\n");
asn_DEF_ContentInfo.free_struct(&asn_DEF_ContentInfo, podpis, 1);
free(podpis);*/
dec_result=ber_decode(0, &asn_DEF_SignedData, (void**)&struktura_podpisu, podpis->content.buf,podpis->content.size);
if(dec_result.code != RC_OK)
{
asn_DEF_ContentInfo.free_struct(&asn_DEF_ContentInfo, podpis, 1);
free(podpis);
asn_DEF_SignedData.free_struct(&asn_DEF_SignedData, struktura_podpisu, 1);
free(struktura_podpisu);
printf("1 nie udalo sie\n");
return -1;
}
SignedAttributes_t *sigattrs=struktura_podpisu->signerInfos.list.array[0]->signedAttrs;
UnsignedAttributes_t *unsigattrs=struktura_podpisu->signerInfos.list.array[0]->unsignedAttrs;
printf("czy sa niepodpisane atrybuty: %p\n", sigattrs);
printf("czy sa podpisane: %p;\n", unsigattrs);
printf("ile atrybutow podpissanych: %i\n",sigattrs->list.count );
int i=0, j=0;
int tab_oid[10];
int num_slot=0;
for(i=0; i<sigattrs->list.count; i++)
{
num_slot=OBJECT_IDENTIFIER_get_arcs( &(sigattrs->list.array[i]->attrType), tab_oid, sizeof(tab_oid[0]), 10);
for(j=0; j<num_slot; j++)
{
printf("%i:%i; ", j, tab_oid[j]);
}
printf("\n");
}
printf("\nniepodpisane\n");
for(i=0; i<unsigattrs->list.count; i++)
{
num_slot=OBJECT_IDENTIFIER_get_arcs( &(unsigattrs->list.array[i]->attrType), tab_oid, sizeof(tab_oid[0]), 10);
for(j=0; j<num_slot; j++)
{
printf("%i:%i; ", j, tab_oid[j]);
}
printf("\n");
}
printf("udalo sie");
asn_DEF_ContentInfo.free_struct(&asn_DEF_ContentInfo, podpis, 1);
free(podpis);
asn_DEF_SignedData.free_struct(&asn_DEF_SignedData, struktura_podpisu, 1);
free(struktura_podpisu);
return 0;
}
int main() {
initialise_termination_handler();
int deviceDescriptor = open ("/dev/video0", O_RDWR /* required */ | O_NONBLOCK, 0);
if (deviceDescriptor == -1) {
std::cout << "Unable to open device\n";
return 1;
}
bool isAutofocusAvailable = isLogitechAutofocusModeSupported(deviceDescriptor);
if (isAutofocusAvailable) {
}
int announce_socket;
in_addr iaddr;
memset(&iaddr, 0, sizeof(struct in_addr));
announce_socket = socket(AF_INET, SOCK_DGRAM, 0);
if ( announce_socket < 0 ) {
perror("Error creating socket");
exit(0);
}
int one = 1;
int ret = setsockopt(announce_socket, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one));
if (ret < 0) {
perror("setsockopt");
return 1;
}
sockaddr_in saddr;
memset(&saddr, 0, sizeof(struct sockaddr_in));
saddr.sin_family = AF_INET;
saddr.sin_port = htons(CAMERA_ANNOUNCE_PORT);
saddr.sin_addr.s_addr = INADDR_ANY; // bind socket to any interface
int status = bind(announce_socket, (struct sockaddr *)&saddr, sizeof(sockaddr_in));
if ( status < 0 )
perror("Error binding socket to interface"), exit(0);
ip_mreq mreq;
memset(&mreq,0,sizeof(mreq));
mreq.imr_multiaddr.s_addr=inet_addr(CAMERA_ANNOUNCE_GROUP);
mreq.imr_interface.s_addr=INADDR_ANY;
if (setsockopt(announce_socket,IPPROTO_IP,IP_ADD_MEMBERSHIP,&mreq,sizeof(mreq)) < 0) {
perror("setsockopt");
exit(1);
}
Autofocus autofocus(deviceDescriptor);
while (running == 1 && !autofocus.isFocusComplete()) {
BufferReference readyBuffer;
memset(&readyBuffer,0,sizeof(readyBuffer));
msghdr socket_message;
memset(&socket_message,0,sizeof(socket_message));
int result;
std::vector<char> data(1024);
result = recvfrom(announce_socket,data.data(),data.size(),0,NULL,NULL);
if (result <= 0) {
perror("recvfrom");
break;
}
timespec tp;
clock_gettime(CLOCK_MONOTONIC,&tp);
void* input_pointer = &readyBuffer;
ber_decode(0,&asn_DEF_BufferReference,&input_pointer,data.data(),result);
// timeval capture_time{readyBuffer.timestamp_seconds,readyBuffer.timestamp_microseconds};
std::string buffer_name = "/" + get_name_of_buffer(readyBuffer.sequence);
int buffer_descriptor = shm_open(buffer_name.c_str(), O_RDONLY, 0);
if (buffer_descriptor == -1) {
std::cout << "Can not open shared buffer file " << buffer_name << std::endl;
continue;
}
size_t buffer_length = readyBuffer.width*readyBuffer.height*2;
void* pointer = mmap(NULL,buffer_length, PROT_READ,MAP_PRIVATE,buffer_descriptor,0);
close(buffer_descriptor);
if (pointer == MAP_FAILED) {
std::cout << "Can not map shared buffer " << buffer_name << std::endl;
continue;
}
timeval frame_timestamp {readyBuffer.timestamp_seconds,readyBuffer.timestamp_microseconds};
yuy2::view_t frame = boost::gil::interleaved_view(readyBuffer.width,readyBuffer.height,static_cast<yuy2::ptr_t>(pointer),readyBuffer.width*2);
autofocus.submitFrame(frame_timestamp, frame);
munmap(pointer,buffer_length);
}
close(announce_socket);
return 0;
}
MmsIndication
mmsClient_parseWriteMultipleItemsResponse(ByteBuffer* message, int itemCount, LinkedList* accessResults)
{
MmsPdu_t* mmsPdu = 0;
MmsIndication retVal = MMS_OK;
asn_dec_rval_t rval;
rval = ber_decode(NULL, &asn_DEF_MmsPdu,
(void**) &mmsPdu, ByteBuffer_getBuffer(message), ByteBuffer_getSize(message));
if (rval.code != RC_OK) {
retVal = MMS_ERROR;
goto cleanUp;
}
if (DEBUG) xer_fprint(stdout, &asn_DEF_MmsPdu, mmsPdu);
if (mmsPdu->present == MmsPdu_PR_confirmedResponsePdu) {
if (mmsPdu->choice.confirmedResponsePdu.confirmedServiceResponse.present == ConfirmedServiceResponse_PR_write)
{
WriteResponse_t* response =
&(mmsPdu->choice.confirmedResponsePdu.confirmedServiceResponse.choice.write);
if (response->list.count == itemCount) {
int i;
*accessResults = LinkedList_create();
for (i = 0; i < itemCount; i++) {
MmsValue* value;
if (response->list.array[i]->present == WriteResponse__Member_PR_success) {
MmsDataAccessError error;
value = MmsValue_newDataAccessError(DATA_ACCESS_ERROR_SUCCESS);
}
else {
long errorCode;
asn_INTEGER2long(&response->list.array[i]->choice.failure, &errorCode);
value = MmsValue_newDataAccessError((MmsDataAccessError) errorCode);
}
LinkedList_add(*accessResults, (void*) value);
}
}
else
retVal = MMS_ERROR;
}
else {
retVal = MMS_ERROR;
}
}
else {
retVal = MMS_ERROR;
}
cleanUp:
asn_DEF_MmsPdu.free_struct(&asn_DEF_MmsPdu, mmsPdu, 0);
return retVal;
}
void
mmsServer_handleGetNamedVariableListAttributesRequest(
MmsServerConnection connection,
uint8_t* buffer, int bufPos, int maxBufPos,
uint32_t invokeId,
ByteBuffer* response)
{
GetNamedVariableListAttributesRequest_t* request = 0;
asn_dec_rval_t rval = ber_decode(NULL, &asn_DEF_GetNamedVariableListAttributesRequest,
(void**) &request, buffer + bufPos, maxBufPos - bufPos);
if (rval.code != RC_OK) {
mmsServer_writeMmsRejectPdu(&invokeId, MMS_ERROR_REJECT_INVALID_PDU, response);
return;
}
if (request->present == ObjectName_PR_domainspecific) {
char domainName[65];
char itemName[65];
if ((request->choice.domainspecific.domainId.size > 64) ||
(request->choice.domainspecific.itemId.size > 64)) {
mmsServer_createConfirmedErrorPdu(invokeId, response, MMS_ERROR_ACCESS_OTHER);
goto exit_function;
}
StringUtils_createStringFromBufferInBuffer(domainName, request->choice.domainspecific.domainId.buf,
request->choice.domainspecific.domainId.size);
StringUtils_createStringFromBufferInBuffer(itemName, request->choice.domainspecific.itemId.buf,
request->choice.domainspecific.itemId.size);
MmsDevice* mmsDevice = MmsServer_getDevice(connection->server);
MmsDomain* domain = MmsDevice_getDomain(mmsDevice, domainName);
if (domain != NULL) {
MmsNamedVariableList variableList =
MmsDomain_getNamedVariableList(domain, itemName);
if (variableList != NULL)
createGetNamedVariableListAttributesResponse(invokeId, response, variableList);
else
mmsServer_createConfirmedErrorPdu(invokeId, response, MMS_ERROR_ACCESS_OBJECT_NON_EXISTENT);
}
else
mmsServer_createConfirmedErrorPdu(invokeId, response, MMS_ERROR_ACCESS_OBJECT_NON_EXISTENT);
}
#if (MMS_DYNAMIC_DATA_SETS == 1)
else if (request->present == ObjectName_PR_aaspecific) {
char listName[65];
if (request->choice.aaspecific.size > 64) {
mmsServer_createConfirmedErrorPdu(invokeId, response, MMS_ERROR_ACCESS_OTHER);
goto exit_function;
}
StringUtils_createStringFromBufferInBuffer(listName, request->choice.aaspecific.buf,
request->choice.aaspecific.size);
MmsNamedVariableList varList = MmsServerConnection_getNamedVariableList(connection, listName);
if (varList != NULL)
createGetNamedVariableListAttributesResponse(invokeId, response, varList);
else
mmsServer_createConfirmedErrorPdu(invokeId, response, MMS_ERROR_ACCESS_OBJECT_NON_EXISTENT);
}
#endif /* (MMS_DYNAMIC_DATA_SETS == 1) */
else {
mmsServer_createConfirmedErrorPdu(invokeId, response, MMS_ERROR_ACCESS_OBJECT_ACCESS_UNSUPPORTED);
}
exit_function:
asn_DEF_GetVariableAccessAttributesRequest.free_struct(&asn_DEF_GetNamedVariableListAttributesRequest,
request, 0);
}
static void
check(int is_ok, uint8_t *buf, size_t size, size_t consumed) {
T1_t t, *tp;
void *tpp = &tp;
asn_dec_rval_t rval;
asn_enc_rval_t erval;
int ret;
int i;
tp = memset(&t, 0, sizeof(t));
fprintf(stderr, "Buf %p\n", buf);
rval = ber_decode(0, &asn_DEF_T1, (void **)tpp, buf, size);
fprintf(stderr, "Returned code %d, consumed %d\n",
(int)rval.code, (int)rval.consumed);
if(is_ok) {
assert(rval.code == RC_OK);
assert(rval.consumed == (size_t)consumed);
assert(t.a.size == 2);
assert(t.b.present == b_PR_n);
assert(t.b.choice.n.size == 1);
assert(t.b.choice.n.buf[0] == 'i');
assert(t.c.size == 1);
assert(t.c.buf[0] == 'x');
} else {
if(rval.code == RC_OK) {
assert(t.a.size != 2
|| t.b.present != b_PR_n
|| t.b.choice.n.size != 1
|| t.c.size != 1
);
}
assert(rval.consumed <= (size_t)consumed);
ASN_STRUCT_RESET(asn_DEF_T1, tp);
return;
}
fprintf(stderr, "=> Re-creating using DER encoder <=\n");
/*
* Try to re-create using DER encoding.
*/
buf2_pos = 0;
erval = der_encode(&asn_DEF_T1, tp, buf2_fill, 0);
assert(erval.encoded != -1);
if(erval.encoded != sizeof(buf1)) {
printf("%d != %d\n", (int)erval.encoded, (int)sizeof(buf1));
}
assert(erval.encoded == (ssize_t)sizeof(buf1));
for(i = 0; i < (ssize_t)sizeof(buf1); i++) {
if(buf1[i] != buf2[i]) {
fprintf(stderr, "Recreated buffer content mismatch:\n");
fprintf(stderr, "Byte %d, %x != %x (%d != %d)\n",
i,
buf1[i], buf2[i],
buf1[i], buf2[i]
);
}
assert(buf1[i] == buf2[i]);
}
fprintf(stderr, "=== asn_fprint() ===\n");
ret = asn_fprint(stderr, &asn_DEF_T1, tp);
assert(ret == 0);
fprintf(stderr, "=== xer_fprint() ===\n");
ret = xer_fprint(stderr, &asn_DEF_T1, tp);
assert(ret == 0);
fprintf(stderr, "=== EOF ===\n");
ASN_STRUCT_RESET(asn_DEF_T1, tp);
}
/* Student ASN.1 type */
int main(int ac, char **av) {
char buf[1024];/* Temporary buffer*/
asn_dec_rval_t rval; /* Decoder return value */
Student_t *student = 0; /* Type to decode. Note this 01 ! */
FILE *fp;
size_t size;
char *filename;
/* Require a single filename argument */
if(ac != 2) {
fprintf(stderr, "Usage: %s <file.ber>\n", av[0]);
exit(1);
} else {
filename = av[1];
}
/* Open input file as read-only binary */
fp = fopen(filename, "rb");
if(!fp) {
perror(filename);
exit(1);
}
/* Read up to the buffer size */
size = fread(buf, 1, sizeof(buf), fp);
fclose(fp);
if(!size) {
fprintf(stderr, "%s: Empty or broken\n", filename);
exit(1);
}
/* Decode the input buffer as Student type */
rval = ber_decode(0, &asn_DEF_Student, (void **)&student, buf, size);
printf("\nstudent Details\n");
printf("----------------\n");
printf("Name :%s\n",student->student_name);
printf("Age :%ld\n",student->student_age);
printf("height :%ld\n",student->student_height);
printf("weight :%ld\n",student->student_weight);
printf("marks :%f\n",student->student_marks);
printf("----------------\n");
printf("\nHis Father's Details\n");
printf("------------------\n");
printf(" Name :%s\n",student->student_father.name);
printf(" Occupation :%s\n",student->student_father.occupation);
printf(" Age :%ld\n",student->student_father.age);
printf("------------------\n");
printf("\nHis Mother's Details\n");
printf("------------------\n");
printf(" Name :%s\n",student->student_mother.name);
printf(" Occupation :%s\n",student->student_mother.occupation);
printf(" Age :%ld\n",student->student_mother.age);
printf("----------------\n");
printf("\nGirlFriend's Details\n");
printf("----------------\n");
printf("Name :%s\n",student->student_girlfriend.name);
printf("Age :%ld\n",student->student_girlfriend.age);
printf("Height :%ld\n",student->student_girlfriend.height);
printf("Weight :%ld\n",student->student_girlfriend.weight);
printf("Marks :%f\n",student->student_girlfriend.marks);
printf("----------------\n");
printf("size = %d\n",sizeof(Student_t));
if(rval.code != RC_OK) {
fprintf(stderr, "%s: Broken Student encoding at byte %ld\n", filename,
(long)rval.consumed);
exit(1);
}
/* Print the decoded Student type as XML */
//xer_fprint(stdout, &asn_DEF_Student, student);
return 0; /* Decoding finished successfully */
}
static void
partial_read(uint8_t *buf_0, size_t size) {
T_t t, *tp;
asn_dec_rval_t rval;
size_t i1, i2;
uint8_t *buf_1 = alloca(size);
uint8_t *buf_2 = alloca(size);
uint8_t *buf_3 = alloca(size);
fprintf(stderr, "\nPartial read sequence...\n");
/*
* Divide the space (size) into three blocks in various combinations:
* |<----->i1<----->i2<----->|
* ^ buf_0 ^ buf_0+size
* Try to read block by block.
*/
for(i1 = 0; i1 < size; i1++) {
for(i2 = i1; i2 < size; i2++) {
uint8_t *chunk1 = buf_0;
size_t size1 = i1;
uint8_t *chunk2 = buf_0 + size1;
size_t size2 = i2 - i1;
uint8_t *chunk3 = buf_0 + size1 + size2;
size_t size3 = size - size1 - size2;
fprintf(stderr, "\n%d:{%d, %d, %d}...\n",
(int)size, (int)size1, (int)size2, (int)size3);
memset(buf_1, 0, size);
memset(buf_2, 0, size);
memset(buf_3, 0, size);
memcpy(buf_1, chunk1, size1);
memcpy(buf_2, chunk2, size2);
memcpy(buf_3, chunk3, size3);
tp = memset(&t, 0, sizeof(t));
fprintf(stderr, "=> Chunk 1 (%d):\n", (int)size1);
rval = ber_decode(0, &asn_DEF_T, (void **)&tp,
buf_1, size1);
assert(rval.code == RC_WMORE);
assert(rval.consumed <= size1);
if(rval.consumed < size1) {
int leftover = size1 - rval.consumed;
memcpy(buf_2, buf_1 + rval.consumed, leftover);
memcpy(buf_2 + leftover, chunk2, size2);
size2 += leftover;
}
fprintf(stderr, "=> Chunk 2 (%d):\n", (int)size2);
rval = ber_decode(0, &asn_DEF_T, (void **)&tp,
buf_2, size2);
assert(rval.code == RC_WMORE);
assert(rval.consumed <= size2);
if(rval.consumed < size2) {
int leftover = size2 - rval.consumed;
memcpy(buf_3, buf_2 + rval.consumed, leftover);
memcpy(buf_3 + leftover, chunk3, size3);
size3 += leftover;
}
fprintf(stderr, "=> Chunk 3 (%d):\n", (int)size3);
rval = ber_decode(0, &asn_DEF_T, (void **)&tp,
buf_3, size3);
assert(rval.code == RC_OK);
assert(rval.consumed == size3);
asn_DEF_T.free_struct(&asn_DEF_T, &t, 1);
}
}
}
int
mmsServer_handleGetVariableAccessAttributesRequest(
MmsServerConnection connection,
uint8_t* buffer, int bufPos, int maxBufPos,
uint32_t invokeId,
ByteBuffer* response)
{
int retVal = 0;
GetVariableAccessAttributesRequest_t* request = 0;
asn_dec_rval_t rval; /* Decoder return value */
rval = ber_decode(NULL, &asn_DEF_GetVariableAccessAttributesRequest,
(void**) &request, buffer + bufPos, maxBufPos - bufPos);
if (rval.code == RC_OK) {
if (request->present == GetVariableAccessAttributesRequest_PR_name) {
if (request->choice.name.present == ObjectName_PR_domainspecific) {
Identifier_t domainId = request->choice.name.choice.domainspecific.domainId;
Identifier_t nameId = request->choice.name.choice.domainspecific.itemId;
char* domainIdStr = createStringFromBuffer(domainId.buf, domainId.size);
char* nameIdStr = createStringFromBuffer(nameId.buf, nameId.size);
if (DEBUG_MMS_SERVER)
printf("MMS_SERVER: getVariableAccessAttributes domainId: %s nameId: %s\n", domainIdStr, nameIdStr);
createVariableAccessAttributesResponse(connection, domainIdStr, nameIdStr, invokeId, response);
GLOBAL_FREEMEM(domainIdStr);
GLOBAL_FREEMEM(nameIdStr);
}
#if (CONFIG_MMS_SUPPORT_VMD_SCOPE_NAMED_VARIABLES == 1)
else if (request->choice.name.present == ObjectName_PR_vmdspecific) {
Identifier_t nameId = request->choice.name.choice.vmdspecific;
char* nameIdStr = createStringFromBuffer(nameId.buf, nameId.size);
if (DEBUG_MMS_SERVER) printf("MMS_SERVER: getVariableAccessAttributes (VMD specific) nameId: %s\n", nameIdStr);
createVariableAccessAttributesResponse(connection, NULL, nameIdStr, invokeId, response);
GLOBAL_FREEMEM(nameIdStr);
}
#endif /* (CONFIG_MMS_SUPPORT_VMD_SCOPE_NAMED_VARIABLES == 1) */
else {
if (DEBUG_MMS_SERVER) printf("GetVariableAccessAttributesRequest with name other than domainspecific is not supported!\n");
retVal = -1;
}
}
else {
if (DEBUG_MMS_SERVER) printf("GetVariableAccessAttributesRequest with address not supported!\n");
retVal = -1;
}
}
else {
if (DEBUG_MMS_SERVER) printf("GetVariableAccessAttributesRequest parsing request failed!\n");
retVal = -1;
}
asn_DEF_GetVariableAccessAttributesRequest.free_struct(&asn_DEF_GetVariableAccessAttributesRequest, request, 0);
return retVal;
}
static gssize
abstract_sequence_unmarshall(const struct abstract_sequence_handler_s *h,
GSList ** list, const void *asn1_encoded, gsize asn1_encoded_size,
GError ** err)
{
gssize consumed;
void *result = NULL;
gint i = 0, max = 0;
asn_dec_rval_t decRet;
struct anonymous_sequence_s *abstract_sequence;
GSList *api_result = NULL;
void func_free(void *d)
{
if (!d)
return;
h->clean_ASN1(d, FALSE);
}
if (!asn1_encoded || !list) {
GSETERROR(err, "Invalid parameter");
return -1;
}
asn_codec_ctx_t codecCtx = {0};
codecCtx.max_stack_size = ASN1C_MAX_STACK;
decRet = ber_decode(&codecCtx, h->asn1_descriptor, &(result), asn1_encoded, asn1_encoded_size);
switch (decRet.code) {
case RC_OK:
abstract_sequence = (struct anonymous_sequence_s *) result;
/*fill the list with the content of the array */
for (i = 0, max = abstract_sequence->list.count; i < max; i++) {
void *api_structure;
api_structure = g_malloc0(h->api_size);
if (!h->map_ASN1_to_API(abstract_sequence->list.array[i], api_structure)) {
GSETERROR(err,"Element of type [%s] ASN-to-API conversion error", h->type_name);
if (api_structure)
h->clean_API(api_structure);
abstract_sequence->list.free = &func_free;
asn_set_empty(abstract_sequence);
free(abstract_sequence);
abstract_sequence = NULL;
if (api_result) {
g_slist_foreach(api_result, api_gclean, h->clean_API);
g_slist_free(api_result);
}
return -1;
}
api_result = g_slist_prepend(api_result, api_structure);
}
abstract_sequence->list.free = &func_free;
asn_set_empty(abstract_sequence);
free(abstract_sequence);
abstract_sequence = NULL;
*list = metautils_gslist_precat(*list, api_result);
consumed = decRet.consumed;
return consumed;
case RC_FAIL:
GSETERROR(err, "sequence unmarshalling error (%"G_GSIZE_FORMAT" consumed)", decRet.consumed);
return -1;
case RC_WMORE:
GSETERROR(err, "sequence unmarshalling error (uncomplete)");
return 0;
default:
GSETERROR(err, "Serialisation produced an unknow return code : %d", decRet.code);
return -1;
}
return -1;
}
void
mmsServer_handleWriteRequest(
MmsServerConnection* connection,
uint8_t* buffer, int bufPos, int maxBufPos,
uint32_t invokeId,
ByteBuffer* response)
{
WriteRequest_t* writeRequest = 0;
MmsPdu_t* mmsPdu = 0;
asn_dec_rval_t rval; /* Decoder return value */
rval = ber_decode(NULL, &asn_DEF_MmsPdu, (void**) &mmsPdu, buffer, CONFIG_MMS_MAXIMUM_PDU_SIZE);
if (rval.code != RC_OK) {
mmsServer_writeMmsRejectPdu(&invokeId, MMS_ERROR_REJECT_INVALID_PDU, response);
return;
}
writeRequest = &(mmsPdu->choice.confirmedRequestPdu.confirmedServiceRequest.choice.write);
int numberOfWriteItems = writeRequest->variableAccessSpecification.choice.listOfVariable.list.count;
if (numberOfWriteItems < 1) {
mmsServer_writeMmsRejectPdu(&invokeId, MMS_ERROR_REJECT_REQUEST_INVALID_ARGUMENT, response);
return;
}
if (numberOfWriteItems > CONFIG_MMS_WRITE_SERVICE_MAX_NUMBER_OF_WRITE_ITEMS) {
mmsServer_writeMmsRejectPdu(&invokeId, MMS_ERROR_REJECT_OTHER, response);
return;
}
if (writeRequest->listOfData.list.count != numberOfWriteItems) {
mmsServer_writeMmsRejectPdu(&invokeId, MMS_ERROR_REJECT_REQUEST_INVALID_ARGUMENT, response);
return;
}
MmsDataAccessError accessResults[CONFIG_MMS_WRITE_SERVICE_MAX_NUMBER_OF_WRITE_ITEMS * sizeof(MmsDataAccessError)];
bool sendResponse = true;
int i;
for (i = 0; i < numberOfWriteItems; i++) {
ListOfVariableSeq_t* varSpec =
writeRequest->variableAccessSpecification.choice.listOfVariable.list.array[i];
if (varSpec->variableSpecification.present != VariableSpecification_PR_name) {
accessResults[i] = DATA_ACCESS_ERROR_OBJECT_ACCESS_UNSUPPORTED;
continue;
}
if (varSpec->variableSpecification.choice.name.present != ObjectName_PR_domainspecific) {
accessResults[i] = DATA_ACCESS_ERROR_OBJECT_ACCESS_UNSUPPORTED;
continue;
}
Identifier_t domainId = varSpec->variableSpecification.choice.name.choice.domainspecific.domainId;
char* domainIdStr = createStringFromBuffer(domainId.buf, domainId.size);
MmsDevice* device = MmsServer_getDevice(connection->server);
MmsDomain* domain = MmsDevice_getDomain(device, domainIdStr);
free(domainIdStr);
if (domain == NULL) {
accessResults[i] = DATA_ACCESS_ERROR_OBJECT_NONE_EXISTENT;
continue;
}
Identifier_t nameId = varSpec->variableSpecification.choice.name.choice.domainspecific.itemId;
char* nameIdStr = createStringFromBuffer(nameId.buf, nameId.size);
MmsVariableSpecification* variable = MmsDomain_getNamedVariable(domain, nameIdStr);
if (variable == NULL) {
free(nameIdStr);
accessResults[i] = DATA_ACCESS_ERROR_OBJECT_NONE_EXISTENT;
continue;
}
AlternateAccess_t* alternateAccess = varSpec->alternateAccess;
if (alternateAccess != NULL) {
if (variable->type != MMS_ARRAY) {
free(nameIdStr);
accessResults[i] = DATA_ACCESS_ERROR_OBJECT_ATTRIBUTE_INCONSISTENT;
continue;
}
if (!mmsServer_isIndexAccess(alternateAccess)) {
free(nameIdStr);
accessResults[i] = DATA_ACCESS_ERROR_OBJECT_ACCESS_UNSUPPORTED;
continue;
}
}
Data_t* dataElement = writeRequest->listOfData.list.array[i];
MmsValue* value = mmsMsg_parseDataElement(dataElement);
if (value == NULL) {
free(nameIdStr);
accessResults[i] = DATA_ACCESS_ERROR_OBJECT_ATTRIBUTE_INCONSISTENT;
continue;
}
if (alternateAccess != NULL) {
MmsValue* cachedArray = MmsServer_getValueFromCache(connection->server, domain, nameIdStr);
if (cachedArray == NULL) {
free(nameIdStr);
MmsValue_delete(value);
accessResults[i] = DATA_ACCESS_ERROR_OBJECT_ATTRIBUTE_INCONSISTENT;
continue;
}
int index = mmsServer_getLowIndex(alternateAccess);
MmsValue* elementValue = MmsValue_getElement(cachedArray, index);
if (elementValue == NULL) {
free(nameIdStr);
MmsValue_delete(value);
accessResults[i] = DATA_ACCESS_ERROR_OBJECT_ATTRIBUTE_INCONSISTENT;
continue;
}
if (MmsValue_update(elementValue, value) == false) {
free(nameIdStr);
MmsValue_delete(value);
accessResults[i] = DATA_ACCESS_ERROR_TYPE_INCONSISTENT;
continue;
}
free(nameIdStr);
MmsValue_delete(value);
accessResults[i] = DATA_ACCESS_ERROR_SUCCESS;
continue;
}
MmsDataAccessError valueIndication =
mmsServer_setValue(connection->server, domain, nameIdStr, value, connection);
if (valueIndication == DATA_ACCESS_ERROR_NO_RESPONSE)
sendResponse = false;
accessResults[i] = valueIndication;
MmsValue_delete(value);
free(nameIdStr);
}
if (sendResponse) {
mmsServer_createMmsWriteResponse(connection, invokeId, response, numberOfWriteItems, accessResults);
}
asn_DEF_MmsPdu.free_struct(&asn_DEF_MmsPdu, mmsPdu, 0);
}
