/*
* crypt_sha_one() -
* return:
* thread_p(in):
* src(in):
* src_len(in):
* dest_len_p(out):
* Note:
*/
int
crypt_sha_one (THREAD_ENTRY * thread_p, const char *src, int src_len, char **dest_p, int *dest_len_p)
{
int hash_length;
char *dest = NULL;
char *dest_hex = NULL;
int dest_len;
int dest_hex_len;
int error_status = NO_ERROR;
assert (src != NULL);
if (thread_p == NULL)
{
thread_p = thread_get_thread_entry_info ();
}
*dest_p = NULL;
if (init_gcrypt () != NO_ERROR)
{
return ER_ENCRYPTION_LIB_FAILED;
}
hash_length = gcry_md_get_algo_dlen (GCRY_MD_SHA1);
dest = (char *) db_private_alloc (thread_p, hash_length);
if (dest == NULL)
{
error_status = ER_OUT_OF_VIRTUAL_MEMORY;
goto exit_and_free;
}
dest_len = hash_length;
gcry_md_hash_buffer (GCRY_MD_SHA1, dest, src, src_len);
dest_hex = str_to_hex (thread_p, dest, dest_len, &dest_hex, &dest_hex_len);
if (dest_hex == NULL)
{
error_status = ER_OUT_OF_VIRTUAL_MEMORY;
goto exit_and_free;
}
*dest_p = dest_hex;
*dest_len_p = dest_hex_len;
exit_and_free:
if (dest != NULL)
{
db_private_free_and_init (thread_p, dest);
}
return error_status;
}
/*
* crypt_generate_random_bytes() - Generate random number bytes
* return: error code or NO_ERROR
* thread_p(in): thread context
* dest(out): the generated bytes
* length(in): the length of bytes to generate
* Note:
*/
int
crypt_generate_random_bytes (THREAD_ENTRY * thread_p, char *dest, int length)
{
int error_status = NO_ERROR;
assert (dest != NULL);
if (thread_p == NULL)
{
thread_p = thread_get_thread_entry_info ();
}
if (init_gcrypt () != NO_ERROR)
{
return ER_ENCRYPTION_LIB_FAILED;
}
gcry_randomize (dest, length, GCRY_STRONG_RANDOM);
return error_status;
}
/*
* crypt_crc32() -
* return: error code
* thread_p(in):
* src(in): original message
* src_len(in): length of original message
* dest(out): crc32 result
* Note:
*/
int
crypt_crc32 (THREAD_ENTRY * thread_p, const char *src, int src_len, int *dest)
{
int hash_length;
char *hash_result;
int error_status = NO_ERROR;
assert (src != NULL);
if (thread_p == NULL)
{
thread_p = thread_get_thread_entry_info ();
}
if (init_gcrypt () != NO_ERROR)
{
return ER_ENCRYPTION_LIB_FAILED;
}
hash_length = gcry_md_get_algo_dlen (GCRY_MD_CRC32);
hash_result = (char *) db_private_alloc (thread_p, hash_length);
if (hash_result == NULL)
{
error_status = ER_OUT_OF_VIRTUAL_MEMORY;
goto exit_and_free;
}
gcry_md_hash_buffer (GCRY_MD_CRC32, hash_result, src, src_len);
*dest = htonl (*(UINT32 *) hash_result);
exit_and_free:
if (hash_result != NULL)
{
db_private_free_and_init (thread_p, hash_result);
}
return error_status;
}
int main(int argc, char *argv[])
{
int result;
int pcap_available=0;
char dev_name[20];
signal(SIGUSR2, signal_key);
if(argc>1){
strcpy(dev_name,argv[1]);
}else{
strcpy(dev_name,DEFAULT_PCAP_DEV);
}
init_gcrypt(&gcry_hd);
struct hostent *hp;
sock= socket(AF_INET, SOCK_DGRAM, 0);
server.sin_family = AF_INET;
server.sin_port = htons(PORT_IMU);
hp = gethostbyname(IP_IMU);
if (hp==0) printf("IMU IP no alcanzable");
bcopy((char *)hp->h_addr, (char *)&server.sin_addr,hp->h_length);
const u_char pkt_data[4096];
u_char *param;
int rawSocket = openSocket( dev_name );
if ( rawSocket < 0 )
{
fprintf( stderr, "error opening socket\n" );
return 1;
}
gcry_cipher_setkey (gcry_hd,gcry_key,GCRY_KEYLEN);
printf("\r\nINIT INTERCEPTOR");
fflush(stdout);
while(1){
fd_set readfds;
FD_ZERO( &readfds );
FD_SET( rawSocket, &readfds );
int numFds = select( rawSocket+1, &readfds, NULL, NULL, NULL );
if ( numFds == 1 )
{
int caplen = read( rawSocket, pkt_data, sizeof( pkt_data) );
if(pkt_data!=NULL){
packet_handler(pkt_data,caplen);
}
}
}
printf("\r\n EXITED");
return(0);
}
int main(int argc, char *argv[]){
char output[COMMAND_LEN]="";
char last_output[COMMAND_LEN]="";
char msg[MSG];
int pad=0;
int reads=0;
int sock_atropos=0;
int encendido=0;
int gas=0;
int alabeo=0;
int cabeceo=0;
int guinnada=0;
int pipe_des[2];
char com[20]="";
int analog=0;
char conectado=0;
int rounds=0;
int alive=0;
int ispresent=0;
char * ptok;
int out_gas;
int last_gas;
int send_gas=0;
int send_guinnada=0;
int ignore_round=0;
int transient_present=0;
int config=0;
char device[100]="wlan0";
int result=0;
char errorBuffer[PCAP_ERRBUF_SIZE];
char mode_send=MODE_SEND;
const u_char *pkt_data;
struct pcap_pkthdr *header;
printf("\r\nEMISORA INTERCEPTOR\n\n");
if(argc>1){
if (argv[1][0]=='#'){
strcpy(my_str_conn,&argv[1][1]);
if (argc>2){strcpy(device,argv[1]);}
}else{
strcpy(device,argv[1]);
if(argc>=2){
strcpy(stage, argv[2]);
if(argc>=3){
mode_send=argv[3][0];
}
}
}
}
struct sched_param schedule;
schedule.sched_priority=sched_get_priority_min(SCHED_OTHER);
sched_setscheduler(pthread_self(), SCHED_OTHER,&schedule);
schedule.sched_priority=sched_get_priority_min(SCHED_FIFO);
sched_setscheduler(pthread_self(), SCHED_FIFO,&schedule);
printf("\nSTAGE %s", stage);
printf("\nMODE %c", mode_send);
char log_file_path[128]="";
int hastolog=0;
FILE *fl;
fl = fopen(LOG_FILE_TARGET, "r");
if (fl!=NULL){
int readp=fread(log_file_path,sizeof(char),128,fl);
log_file_path[readp-1]='\0';
fclose(fl);
fl=fopen(log_file_path,"w+");
if(fl!=NULL){hastolog=1; printf("\nLOG DE QUERY ACTIVADO");}
}
printf("\nLOG STATUS %i %s ", hastolog, log_file_path);
if(mode_send!='F'){
pcap_if_t * allAdapters;
pcap_if_t * adapter;
printf("\nABRIENDO PCAP");
fflush(stdout);
result= pcap_findalldevs(&allAdapters, errorBuffer );
printf("\nRespuesta PCAP: %i\n", result);
fflush(stdout);
if(result==0){
for( adapter = allAdapters; adapter != NULL; adapter = adapter->next)
{
//printf("\r\nDetectando %s", adapter->name);
if (strcmp(adapter->name, device)==0){
rfmon_avail=1;
break;
}
}
}
if(rfmon_avail){
init_gcrypt(&gcry_hd);
seq=1;
printf("\nInterfaz en RFMON cargada: %s",adapter->name );
capture = pcap_open_live(device, BUFSIZ, 1, 1000, errorBuffer);
printf("\nResultado: %s\n",pcap_geterr(capture));
if(capture==NULL){printf("\n NULL HANDLER");}
}else{
printf("\nADVERTENCIA: Sin soporte con RFMON");
}
}
printf("\nAbriendo entrenador");
pad=open(CONTROL_DEVICE,O_RDONLY|O_NONBLOCK);
char input='n';
if(pad<=0){
printf("\nNo se encuentran mandos. Continuar?[s/N]:");
scanf ("%c",&input);
if(input!='s'){
exit(-1);
}
}else{
conectado=1;
}
conectado=1;
printf("\nEntrenador en #%i\n",pad);
encendido=0;
ispresent=1;
last_gas=20000;
if(hastolog==1){
sprintf(output, "QQZ%iZ0Z0Z0Z", last_gas);
printf("\n ESCRITO PRIMER LOG %i",fprintf(fl,"%s\n", output ));
fflush(fl);
}
if (mode_send!='F'){
if ((stage[0]=='S')||(stage[0]=='N')){
printf("\nGENERANDO CLAVE");
random_vector(gcry_key,GCRY_KEYLEN);
}
if(stage[0]=='K'){
int loadkey=load_key_from_file(gcry_key);
printf("\nCARGADA CLAVE: %i", loadkey);
if(loadkey!=0){exit(-1);}
}else{
while(notify_key_to_rfmon(gcry_key)!=0){
printf("\nKEY EXANGE FAILED, RETRY");
sleep(1);
}
}
printf("\r\n KEY EXANGE OK");
if (stage[0]=='S'){
printf("\nETAPA GENERAR CLAVE HECHO");
exit(0);
}
if(rfmon_avail){
gcry_cipher_setkey (gcry_hd,gcry_key,GCRY_KEYLEN);
}
}
while(1==1){
reads=read(pad,msg,MSG);
if(rfmon_avail==0){mode_send='F';}
if (reads==MSG){
alive=0;
encendido=1;
analog=msg[5];
//printf("\r\n%i ANALOG %i",msg[7], analog);
if((msg[7]==2)&&(encendido==1)){//gas
gas=((analog+127))*GAS_FACTOR;
if((abs(gas-send_gas)<100000000)||((gas<228600000)&&(gas>20000))||(transient_present==1)){
//printf("\r\n!GAS_SPIKE! %i",abs(gas-last_gas));
//send_gas=gas;
transient_present=0;
send_gas=gas;
}
#ifndef MANDO_ABORDO
else{
printf("!");
}
#endif
}
if((msg[7]==4)&&(encendido==1)){//guiñada
guinnada=-analog*GUINNADA_FACTOR;
}
if((msg[7]==1)&&(encendido==1)){//cabeceo
cabeceo=-analog*CABECEO_FACTOR;
}
if((msg[7]==0)&&(encendido==1)){//alabeo
alabeo=-analog*ALABEO_FACTOR;
}
conectado=1;
}else{
alive++;
}
if(alive>=2000){
alive=0;
close(pad);
if(ispresent==0){
transient_present=1;
}
ispresent=1;
pad=open(CONTROL_DEVICE,O_RDONLY|O_NONBLOCK);
if( access( CONTROL_DEVICE, R_OK ) < 0 ) {
ispresent=0;
}
}
rounds++;
if((encendido==1)&&(conectado==1)&&(ispresent==1)&&(rounds>=ROUND_WINDOW)){
if((send_gas<=GAS_CONFIG)&&(guinnada==0)){
config=1;
}
if(send_gas>GAS_CONFIG){config=0;}
if (config==1){
send_guinnada=0;
// printf("\r\n %i %i ", guinnada,STOP_WITH_YAW);
if(guinnada<=STOP_WITH_YAW){
query("Y0 ",mode_send);
}
if(guinnada>=START_WITH_YAW){
if((rfmon_avail)&&(1==2)){
random_vector(gcry_key,GCRY_KEYLEN);
gcry_cipher_setkey (gcry_hd,gcry_key,GCRY_KEYLEN);
seq=2;
notify_key_to_rfmon(gcry_key);
}
query("Y1000000 ",mode_send);
}
if(hastolog==1){fflush(fl);}
}else{
send_guinnada=guinnada;
}
sprintf(output, "QQZ%iZ%iZ%iZ%iZ", (int)send_gas/1000,(int)alabeo/1000, (int)cabeceo/1000, (int)send_guinnada/1000);
if(hastolog==1){
fprintf(fl,"%s\n", output );
}
printf(".");fflush(stdout);
query(output, mode_send);
rounds=0;
}
//#ifndef MANDO_ABORDO
usleep(500);
//#endif
}
}
/*
* crypt_sha_two() -
* return:
* thread_p(in):
* src(in):
* src_len(in):
* need_hash_len(in):
* dest_p(out)
* dest_len_p(out):
* Note:
*/
int
crypt_sha_two (THREAD_ENTRY * thread_p, const char *src, int src_len, int need_hash_len, char **dest_p, int *dest_len_p)
{
int hash_length;
int algo;
char *dest = NULL;
int dest_len;
char *dest_hex = NULL;
int dest_hex_len;
int error_status = NO_ERROR;
assert (src != NULL);
if (thread_p == NULL)
{
thread_p = thread_get_thread_entry_info ();
}
*dest_p = NULL;
switch (need_hash_len)
{
case 0:
case 256:
algo = GCRY_MD_SHA256;
break;
case 224:
algo = GCRY_MD_SHA224;
break;
case 384:
algo = GCRY_MD_SHA384;
break;
case 512:
algo = GCRY_MD_SHA512;
break;
default:
return NO_ERROR;
}
if (init_gcrypt () != NO_ERROR)
{
return ER_ENCRYPTION_LIB_FAILED;
}
hash_length = gcry_md_get_algo_dlen (algo);
dest_len = hash_length;
dest = (char *) db_private_alloc (thread_p, hash_length);
if (dest == NULL)
{
error_status = ER_OUT_OF_VIRTUAL_MEMORY;
goto exit_and_free;
}
gcry_md_hash_buffer (algo, dest, src, src_len);
dest_hex = str_to_hex (thread_p, dest, dest_len, &dest_hex, &dest_hex_len);
if (dest_hex == NULL)
{
error_status = ER_OUT_OF_VIRTUAL_MEMORY;
goto exit_and_free;
}
*dest_p = dest_hex;
*dest_len_p = dest_hex_len;
exit_and_free:
if (dest != NULL)
{
db_private_free_and_init (thread_p, dest);
}
return error_status;
}
/*
* crypt_aes_default_decrypt() - like mysql's aes_decrypt. Use AES-128/ECB/PKCS7 method.
* return:
* thread_p(in):
* src(in): source string
* src_len(in): the length of source string
* key(in): the encrypt key
* key_len(in): the length of the key
* dest_p(out): the encrypted data. The pointer has to be free by db_private_free
* dest_len_p(out):
* Note:
*/
int
crypt_aes_default_decrypt (THREAD_ENTRY * thread_p, const char *src, int src_len, const char *key, int key_len,
char **dest_p, int *dest_len_p)
{
gcry_error_t i_gcrypt_err;
gcry_cipher_hd_t aes_ctx;
char *dest = NULL;
int dest_len = 0;
char new_key[AES128_KEY_LEN];
int pad, pad_len;
int i;
int error_status = NO_ERROR;
assert (src != NULL);
assert (key != NULL);
if (thread_p == NULL)
{
thread_p = thread_get_thread_entry_info ();
}
*dest_p = NULL;
*dest_len_p = 0;
/* src is not a string encrypted by aes_default_encrypt, return NULL */
if (src_len % AES128_BLOCK_LEN)
{
return NO_ERROR;
}
if (init_gcrypt () != NO_ERROR)
{
return ER_ENCRYPTION_LIB_FAILED;
}
i_gcrypt_err = gcry_cipher_open (&aes_ctx, GCRY_CIPHER_AES, GCRY_CIPHER_MODE_ECB, 0);
if (i_gcrypt_err != GPG_ERR_NO_ERROR)
{
er_set (ER_ERROR_SEVERITY, ARG_FILE_LINE, ER_ENCRYPTION_LIB_FAILED, 1,
crypt_lib_fail_info[CRYPT_LIB_OPEN_CIPHER_ERR]);
return ER_ENCRYPTION_LIB_FAILED;
}
dest = (char *) db_private_alloc (thread_p, src_len * sizeof (char));
if (dest == NULL)
{
error_status = ER_OUT_OF_VIRTUAL_MEMORY;
goto error_and_free;
}
aes_default_gen_key (key, key_len, new_key, AES128_KEY_LEN);
i_gcrypt_err = gcry_cipher_setkey (aes_ctx, new_key, AES128_KEY_LEN);
if (i_gcrypt_err != GPG_ERR_NO_ERROR)
{
er_set (ER_ERROR_SEVERITY, ARG_FILE_LINE, ER_ENCRYPTION_LIB_FAILED, 1,
crypt_lib_fail_info[CRYPT_LIB_SET_KEY_ERR]);
error_status = ER_ENCRYPTION_LIB_FAILED;
goto error_and_free;
}
i_gcrypt_err = gcry_cipher_decrypt (aes_ctx, dest, src_len, src, src_len);
if (i_gcrypt_err != GPG_ERR_NO_ERROR)
{
error_status = ER_ENCRYPTION_LIB_FAILED;
er_set (ER_ERROR_SEVERITY, ARG_FILE_LINE, ER_ENCRYPTION_LIB_FAILED, 1, crypt_lib_fail_info[CRYPT_LIB_CRYPT_ERR]);
goto error_and_free;
}
/* PKCS7 */
if (src_len != 0)
{
pad = dest[src_len - 1];
if (pad > AES128_BLOCK_LEN)
{
/* src is not a string encrypted by aes_default_encrypt, return NULL */
if (dest != NULL)
{
db_private_free_and_init (thread_p, dest);
}
goto error_and_free;
}
i = src_len - 2;
pad_len = 1;
while ((i >= 0) && (dest[i] == pad))
{
pad_len++;
i--;
}
if ((pad_len >= pad))
{
pad_len = pad;
dest_len = src_len - pad_len;
}
else
{
/* src is not a string encrypted by aes_default_encrypt, return NULL */
if (dest != NULL)
{
db_private_free_and_init (thread_p, dest);
}
goto error_and_free;
}
}
*dest_p = dest;
*dest_len_p = dest_len;
error_and_free:
if ((dest != NULL) && (error_status != NO_ERROR))
{
db_private_free_and_init (thread_p, dest);
}
gcry_cipher_close (aes_ctx);
return error_status;
}
/*
* crypt_aes_default_encrypt() - like mysql's aes_encrypt. Use AES-128/ECB/PKCS7 method.
* return:
* thread_p(in):
* src(in): source string
* src_len(in): the length of source string
* key(in): the encrypt key
* key_len(in): the length of the key
* dest_p(out): the encrypted data. The pointer has to be free by db_private_free
* dest_len_p(out):
* Note:
*/
int
crypt_aes_default_encrypt (THREAD_ENTRY * thread_p, const char *src, int src_len, const char *key, int key_len,
char **dest_p, int *dest_len_p)
{
gcry_error_t i_gcrypt_err;
gcry_cipher_hd_t aes_ctx;
char new_key[AES128_KEY_LEN];
int pad;
int padding_src_len;
char *padding_src = NULL;
char *dest = NULL;
int error_status = NO_ERROR;
assert (src != NULL);
assert (key != NULL);
if (thread_p == NULL)
{
thread_p = thread_get_thread_entry_info ();
}
*dest_p = NULL;
*dest_len_p = 0;
if (init_gcrypt () != NO_ERROR)
{
return ER_ENCRYPTION_LIB_FAILED;
}
i_gcrypt_err = gcry_cipher_open (&aes_ctx, GCRY_CIPHER_AES, GCRY_CIPHER_MODE_ECB, 0);
if (i_gcrypt_err != GPG_ERR_NO_ERROR)
{
er_set (ER_ERROR_SEVERITY, ARG_FILE_LINE, ER_ENCRYPTION_LIB_FAILED, 1,
crypt_lib_fail_info[CRYPT_LIB_OPEN_CIPHER_ERR]);
return ER_ENCRYPTION_LIB_FAILED;
}
/* PKCS7 */
if ((src_len % AES128_BLOCK_LEN) == 0)
{
pad = AES128_BLOCK_LEN;
padding_src_len = src_len + pad;
}
else
{
padding_src_len = ceil ((double) src_len / AES128_BLOCK_LEN) * AES128_BLOCK_LEN;
pad = padding_src_len - src_len;
}
padding_src = (char *) db_private_alloc (thread_p, padding_src_len);
if (padding_src == NULL)
{
error_status = ER_OUT_OF_VIRTUAL_MEMORY;
goto exit_and_free;
}
memcpy (padding_src, src, src_len);
memset (padding_src + src_len, pad, pad);
aes_default_gen_key (key, key_len, new_key, AES128_KEY_LEN);
i_gcrypt_err = gcry_cipher_setkey (aes_ctx, new_key, AES128_KEY_LEN);
if (i_gcrypt_err != GPG_ERR_NO_ERROR)
{
error_status = ER_ENCRYPTION_LIB_FAILED;
er_set (ER_ERROR_SEVERITY, ARG_FILE_LINE, ER_ENCRYPTION_LIB_FAILED, 1,
crypt_lib_fail_info[CRYPT_LIB_SET_KEY_ERR]);
goto exit_and_free;
}
dest = (char *) db_private_alloc (thread_p, padding_src_len);
if (dest == NULL)
{
error_status = ER_OUT_OF_VIRTUAL_MEMORY;
goto exit_and_free;
}
i_gcrypt_err = gcry_cipher_encrypt (aes_ctx, dest, padding_src_len, padding_src, padding_src_len);
if (i_gcrypt_err != GPG_ERR_NO_ERROR)
{
er_set (ER_ERROR_SEVERITY, ARG_FILE_LINE, ER_ENCRYPTION_LIB_FAILED, 1, crypt_lib_fail_info[CRYPT_LIB_CRYPT_ERR]);
error_status = ER_ENCRYPTION_LIB_FAILED;
goto exit_and_free;
}
*dest_len_p = padding_src_len;
*dest_p = dest;
exit_and_free:
if (padding_src != NULL)
{
db_private_free_and_init (thread_p, padding_src);
}
if ((dest != NULL) && (error_status != NO_ERROR))
{
db_private_free_and_init (thread_p, dest);
}
gcry_cipher_close (aes_ctx);
return error_status;
}
