int nbs_pack_envelope(void *packet, unsigned int dataid, size_t data_size){
/*
* The envelope of the data packet contains:
*
* 4 bytes for the data id
* 4 bytes for the size
* 4 bytes for the checksum of the data[]
* the data[] (see the function above for the case of fpath or nbs.c for
* the case of the file content.)
*
* This function assumes that all the packet data is already
* stored in &packet[NBS_ENVELOPE_SIZE] and then it fills the envelope.
*/
unsigned char *p;
unsigned char *data;
uint32_t checksum = 0;
p = (unsigned char*)packet;
data = &p[NBS_ENVELOPE_SIZE];
pack_uint32(p, dataid, 0);
pack_uint32(p, data_size, 4);
checksum = calc_checksum(data, data_size);
pack_uint32(p, checksum, 8);
return(0);
}
bool handle_ICMP_packet(packet_info_t *pi) {
debug_println("Packet is ICMP");
byte *icmp_packet = pi->packet+IPV4_HEADER_OFFSET+IPV4_HEADER_LENGTH;
struct icmp_echo_hdr *icmp_hdr = (void *)pi->packet+IPV4_HEADER_OFFSET+IPV4_HEADER_LENGTH;
/*unsigned i;
for (i = 0; i < pi->len-IPV4_HEADER_OFFSET-IPV4_HEADER_LENGTH; i++)
printf("(%d %02X) ", i, (int)*(icmp_packet+i));
printf("\n");*/
if (calc_checksum(icmp_packet, pi->len-IPV4_HEADER_OFFSET-IPV4_HEADER_LENGTH)) {
debug_println("ICMP checksum failed, dropping packet!");
return 1;
}
uint8_t type = ICMPH_TYPE(icmp_hdr);
switch (type) {
case ICMP_TYPE_ECHO_REQUEST: ICMPH_TYPE_SET(icmp_hdr, 0);
break;
case ICMP_TYPE_ECHO_REPLY: handle_ping_reply(pi); return 1;
default: debug_println("Dropping packet: type %d unknown.", type); return 1;
}
ICMPH_CHKSUM_SET(icmp_hdr, 0);
ICMPH_CHKSUM_SET(icmp_hdr, htons(calc_checksum(icmp_packet, pi->len-IPV4_HEADER_OFFSET-IPV4_HEADER_LENGTH)));
/*for (i = 0; i < pi->len-IPV4_HEADER_OFFSET-IPV4_HEADER_LENGTH; i++)
printf("(%d %02X) ", i, (int)*(icmp_packet+i));
printf("\n");*/
return 0;
}
void
UBX::send_message(const uint16_t msg, const uint8_t *payload, const uint16_t length)
{
ubx_header_t header = {UBX_SYNC1, UBX_SYNC2};
ubx_checksum_t checksum = {0, 0};
// Populate header
header.msg = msg;
header.length = length;
// Calculate checksum
calc_checksum(((uint8_t *)&header) + 2, sizeof(header) - 2, &checksum); // skip 2 sync bytes
if (payload != nullptr) {
calc_checksum(payload, length, &checksum);
}
// Send message
write(_fd, (const void *)&header, sizeof(header));
if (payload != nullptr) {
write(_fd, (const void *)payload, length);
}
write(_fd, (const void *)&checksum, sizeof(checksum));
}
void send_ping(router_t *router, addr_ip_t dest_ip, addr_ip_t src_ip, uint16_t id, uint16_t count) {
int len = IPV4_HEADER_LENGTH+8;
byte *payload = malloc_or_die(len*sizeof(byte)); //Free'd (below)
struct ip_hdr *iphdr = (void *)payload;
IPH_VHLTOS_SET(iphdr, 4, 5, 0);
IPH_LEN_SET(iphdr, htons(len));
IPH_ID_SET(iphdr, 0);
IPH_OFFSET_SET(iphdr, 0);
IPH_TTL_SET(iphdr, 64);
IPH_PROTO_SET(iphdr, 1);
iphdr->src.addr = src_ip;
iphdr->dest.addr = dest_ip;
IPH_CHKSUM_SET(iphdr, 0);
IPH_CHKSUM_SET(iphdr, htons(calc_checksum(payload, IPV4_HEADER_LENGTH)));
struct icmp_echo_hdr *pihdr = (void *)payload+IPV4_HEADER_LENGTH;
ICMPH_TYPE_SET(pihdr, ICMP_TYPE_ECHO_REQUEST);
ICMPH_CODE_SET(pihdr, 0);
pihdr->id = id;
pihdr->seqno = count;
ICMPH_CHKSUM_SET(pihdr, 0);
ICMPH_CHKSUM_SET(pihdr, htons(calc_checksum(payload+IPV4_HEADER_LENGTH, 8)));
send_packet(payload, src_ip, dest_ip, len, FALSE, FALSE);
free(payload);
}
int OFB_push(unsigned char inbuffer[])
{
static int overflowed=0;
if(ofb_size!=OVERFLOWBUFFERDEPTH)
{
unsigned char checksum;
checksum=calc_checksum(inbuffer);
//ofb_buffer[ofb_head][0]='1';
ofb_buffer[ofb_head][DMABUFFERSIZE+4]=checksum;
ofb_head++;
if(ofb_head>=OVERFLOWBUFFERDEPTH)
{
ofb_head=0;
}
ofb_size++;
return 1;
}
else
{
printf("OverFlow Buffer was overflowed\r\n");
overflowed++;
return 0;
}
}
static ssize_t kgamma_b_store(struct device *dev, struct device_attribute *attr,
const char *buf, size_t count)
{
int kgamma[10];
int i;
sscanf(buf, "%d %d %d %d %d %d %d %d %d %d",
&kgamma[0], &kgamma[1], &kgamma[2], &kgamma[3],
&kgamma[4], &kgamma[5], &kgamma[6], &kgamma[7],
&kgamma[8], &kgamma[9]);
if (calc_checksum(kgamma)) {
kgamma[0] = 0xd4;
for (i=0; i<10; i++) {
//pr_info("kgamma_b_p [%d] => %d \n", i, kgamma[i]);
faux123_power_on_set_1[9].payload[i] = kgamma[i];
}
kgamma[0] = 0xd5;
for (i=0; i<10; i++) {
//pr_info("kgamma_b_n [%d] => %d \n", i, kgamma[i]);
faux123_power_on_set_1[10].payload[i] = kgamma[i];
}
}
return count;
}
BOOL
ant_write_message(UCHAR type, UCHAR *data, UCHAR len)
{
int r;
int actual;
BOOL res = FALSE;
UCHAR buff[ 256 ];
buff[ 0 ] = MESG_TX_SYNC;
buff[ 1 ] = len;
buff[ 2 ] = type;
memcpy(buff + 3, data, len);
buff[ 3 + len ] = calc_checksum(buff, (3 + len));
if (ih_cfg != NULL)
{
r = libusb_bulk_transfer(ih_cfg->dev_handle,
(1 | LIBUSB_ENDPOINT_OUT),
buff,
3 + len + 1,
&actual,
0);
if ((r == 0) && (actual == (3 + len + 1)))
{
res = TRUE;
}
else
{
printf("Writing FAILED! r = %d actual=%d\n", r, actual);
}
}
return res;
}
static ssize_t headphone_gain_store(struct kobject *kobj,
struct kobj_attribute *attr, const char *buf, size_t count)
{
unsigned int lval, rval, chksum;
sscanf(buf, "%u %u %u", &lval, &rval, &chksum);
#ifdef CONFIG_MACH_LGE
if (lge_stweaks_control)
return count;
#endif
if (calc_checksum(lval, rval, chksum)) {
#ifdef CONFIG_MACH_LGE
lge_snd_ctrl_locked = 0;
#endif
taiko_write(fauxsound_codec_ptr,
TAIKO_A_CDC_RX1_VOL_CTL_B2_CTL, lval);
taiko_write(fauxsound_codec_ptr,
TAIKO_A_CDC_RX2_VOL_CTL_B2_CTL, rval);
#ifdef CONFIG_MACH_LGE
lge_snd_ctrl_locked = 1;
#endif
}
return count;
}
BOOL
c2pm3_send_command(UCHAR cmd, UCHAR *data, UCHAR len)
{
int r;
int actual;
BOOL res = FALSE;
UCHAR buff[ 21 ];
memset(buff, 0, 21);
buff[ 0 ] = 0x01;
buff[ 1 ] = START_FLAG;
buff[ 2 ] = cmd;
memcpy(buff + 3, data, len);
buff[ 3 + len ] = calc_checksum(buff + 2, (1 + len));
buff[ 4 + len ] = STOP_FLAG;
if (ih_cfg != NULL)
{
r = libusb_bulk_transfer(ih_cfg->dev_handle,
(4 | LIBUSB_ENDPOINT_OUT),
buff,
21,
&actual,
0);
if ((r == 0) && (actual == 21))
{
res = TRUE;
}
else
{
printf("Writing FAILED! r = %d actual=%d\n", r, actual);
}
}
return res;
}
unsigned int wac_generate_random_pin(char *my_pin)
{
FILE *fHandle;
unsigned int buf;
char pin[9];
fHandle = fopen("/dev/urandom", "rb");
if (NULL == fHandle) {
printf("Could not open /dev/urandom.\n");
return -1;
}
fread(&buf, 1, sizeof(unsigned int), fHandle);
fclose(fHandle);
buf %= 10000000;
buf = buf * 10 + calc_checksum(buf);
printf("Random PIN = %d\n", buf);
snprintf(pin, 9, "%08d", buf);
memcpy(my_pin, pin, 8);
return buf;
}
void _GPS::request(NMEA_types type, byte mode, byte rate, boolean chksum)
{
// update template with user-defined NMEA type
if (GGA == type) buffer_out[10] = '0';
else if(GLL == type) buffer_out[10] = '1';
else if(GSA == type) buffer_out[10] = '2';
else if(GSV == type) buffer_out[10] = '3';
else if(RMC == type) buffer_out[10] = '4';
else if(VTG == type) buffer_out[10] = '5';
// update for user-defined mode
buffer_out[13] = mode + 48;
// update for user-defined update rate (TODO: modify to allow rates > 10)
buffer_out[16] = rate + 48;
// update user-defined checksum status
if(chksum)
buffer_out[19] = '1';
else
buffer_out[19] = '0';
calc_checksum();
send(buffer_out, 25);
}
static void send_command(unsigned char *command, int command_length)
{
int retry = 0, sent;
unsigned char sbuf[128];
/* Prepare the send buffer */
sbuf[0] = PW_COMMAND_START_BYTE;
sbuf[1] = (unsigned char)(command_length);
memcpy(sbuf+2, command, command_length);
command_length += 2;
/* Add checksum */
sbuf[command_length] = calc_checksum(sbuf);
command_length += 1;
upsdebug_hex (3, "send_command", sbuf, command_length);
while (retry++ < PW_MAX_TRY) {
if (retry == PW_MAX_TRY) {
ser_send_char(upsfd, 0x1d); /* last retry is preceded by a ESC.*/
usleep(250000);
}
sent = ser_send_buf(upsfd, sbuf, command_length);
if (sent == command_length) {
return;
}
}
}
static void *do_request_wait(void *args)
{
NNTI_result_t rc;
selfsend_args *ssa;
double wait_time=0.0;
NNTI_alloc(&trans_hdl, NNTI_REQUEST_BUFFER_SIZE, 10, NNTI_RECV_QUEUE, &queue_mr);
NNTI_create_work_request(&queue_mr, &queue_wr);
/* client is waiting for us to initialize */
pthread_barrier_wait(&barrier3);
/* the client sends a message here */
NNTI_wait(&queue_wr, -1, &queue_status);
ssa=(selfsend_args *)queue_status.start;
if (ssa->chksum != calc_checksum((const char *)&ssa->data, sizeof(data_t))) {
fprintf(stdout, "checksum failure in the request thread");
success=false;
}
pthread_barrier_wait(&barrier3);
NNTI_destroy_work_request(&queue_wr);
NNTI_free(&queue_mr);
return(NULL);
}
/*
* Packets headed towards the Internet, outside interface address placed
* as source of packet.
*/
unsigned int apply_eip_snat(const struct net_device *out, struct sk_buff *skb)
{
struct iphdr *iph = NULL;
struct extended_ip *ipopt = NULL;
//if(!out || strcmp(out->name, "eth0") != 0){
if(!out || strcmp(out->name, nic_name) != 0){
return NF_ACCEPT;
}
iph = ip_hdr(skb);
if(!iph)
return NF_ACCEPT;
if(iph->ihl == 8){
//ipopt = (void *)skb->data + sizeof(struct iphdr);
ipopt = (char *)(skb->data + sizeof(struct iphdr));
if(ipopt->optionid == ENIP_MAGIC){
ipopt->esp = 1;
ipopt->extended_saddr = iph->saddr;
iph->saddr = outside_ip;
ipopt->esp = 1;
iph->check = 0;
calc_checksum(skb);
}
}
return NF_ACCEPT;
}
void ist_default(INDEX_SECTOR_T *IndexSector)
{
memset(IndexSector, 0, INDEX_SECTOR_SIZE);
IndexSector->Tag = ENDIAN_INT(TAG_SECTOR_TABLE);
//IndexSector->GSensorSensitivityVer = ENDIAN_SHORT(1);
//IndexSector->GSensorSensitivityAlgorithm = ENDIAN_SHORT(1);
IndexSector->Checksum = calc_checksum( (unsigned long *)IndexSector, (INDEX_SECTOR_SIZE-4)/4);
}
int iphdr_check(const void *data)
{
ip_fragment_t *frag = (ip_fragment_t *)data;
__u8 head_length;
if ((head_length = frag->head->ihl) < 5)
return -1;
return calc_checksum(frag) ? 0 : -1;
}
/* Verify a datastream with trailing 6-bit CRC checksum
* Returns 1 if checksum is valid, 0 otherwise
* Author: Maurijn Neumann
*/
int verify_checksum(unsigned char* data, int len) {
//Declare variables
unsigned char checksum;
//Assume checksum is appended at end, calculate
checksum = calc_checksum(data, len);
//Verify
return (checksum == 0x00);
}
void write_checksum (char* tarname, int etype) {
char checksum[7];
char header[265];
FILE* tar;
tar = fopen(tarname, "rb+");
fread(header, 265, 1, tar);
calc_checksum(header, checksum);
fseek(tar, 148, SEEK_SET);
fputs(checksum, tar);
fclose(tar);
}
void validate_sysparam()
{
uint8_t checksum = calc_checksum(&sysparam, sizeof(sysparam));
if ((sysparam.header.magic != SYSPARAM_MAGIC) ||
(sysparam.header.checksum != checksum) ||
(sysparam.header.version != SYSPARAM_VERSION)) {
printf("------------init- \n");
memcpy((void*)&sysparam, (const void*)&initial, sizeof(sysparam));
save_sysparam();
}
}
static void readonly_check_dtor(struct enif_tmp_obj_t* o)
{
struct readonly_check_t* obj = (struct readonly_check_t*) o;
unsigned chksum = calc_checksum(obj->ptr, obj->size);
if (chksum != obj->checksum) {
fprintf(stderr, "\r\nReadonly data written by NIF, checksums differ"
" %x != %x\r\nABORTING\r\n", chksum, obj->checksum);
abort();
}
erts_free(obj->hdr.allocator, obj);
}
void send_ack(uint8_t seq)
{
const uint8_t size = 0x05;
uint8_t msg[size];
msg[0] = IdAckMessage;
msg[1] = size;
msg[2] = sequence++;
msg[3] = seq;
msg[4] = calc_checksum(msg);
uart0_write_buffer(msg, size);
}
int main(int argc, char *argv[])
{
NNTI_result_t rc;
selfsend_args *ssa;
char server_url[NNTI_URL_LEN];
logger_init(LOG_ERROR, NULL);
pthread_barrier_init(&barrier2, NULL, 2);
pthread_barrier_init(&barrier3, NULL, 3);
rc=NNTI_init(NNTI_DEFAULT_TRANSPORT, NULL, &trans_hdl);
rc=NNTI_get_url(&trans_hdl, server_url, NNTI_URL_LEN);
launch_wait_threads();
pthread_barrier_wait(&barrier3);
rc=NNTI_connect(
&trans_hdl,
server_url,
5000,
&server_hdl);
pthread_barrier_wait(&barrier2);
rc=NNTI_alloc(&trans_hdl, NNTI_REQUEST_BUFFER_SIZE, 1, NNTI_SEND_SRC, &send_mr);
ssa=(selfsend_args *)NNTI_BUFFER_C_POINTER(&send_mr);
ssa->data.int_val =10;
ssa->data.float_val =10.0;
ssa->data.double_val=10.0;
ssa->chksum=calc_checksum((const char *)&ssa->data, sizeof(data_t));
rc=NNTI_send(&server_hdl, &send_mr, &recv_mr, &send_wr);
rc=NNTI_wait(&send_wr, 5000, &send_status);
rc=NNTI_send(&server_hdl, &send_mr, NULL, &send_wr);
rc=NNTI_wait(&send_wr, 5000, &send_status);
pthread_barrier_wait(&barrier3);
NNTI_free(&send_mr);
join_wait_threads();
if (success)
std::cout << "\nEnd Result: TEST PASSED" << std::endl;
else
std::cout << "\nEnd Result: TEST FAILED" << std::endl;
return (success ? 0 : 1 );
}
struct pkt *make_pkt(int seqnum, char data[MSGSIZE])
{
// make_pkt: Returns a pointer to a newly initialized packet
struct pkt *gen_pkt;
gen_pkt = new struct pkt;
gen_pkt->seqnum = seqnum;
gen_pkt->acknum = 0;
for(int i = 0; i < sizeof(gen_pkt->payload) / sizeof(char); i++) {
gen_pkt->payload[i] = data[i];
}
gen_pkt->checksum = calc_checksum(gen_pkt);
return gen_pkt;
}
void verify_checksum(char *name)
{
printf("%s: ", name);
calc_checksum(name);
if (!checksum_found) {
printf("No checksum present.\n");
exit(1);
}
if (!checksum_match)
printf("FAIL!\n");
else
printf("OK!\n");
}
static void add_readonly_check(ErlNifEnv* env, unsigned char* ptr, unsigned sz)
{
ErtsAlcType_t allocator = is_proc_bound(env) ? ERTS_ALC_T_TMP : ERTS_ALC_T_NIF;
struct readonly_check_t* obj = erts_alloc(allocator,
sizeof(struct readonly_check_t));
obj->hdr.allocator = allocator;
obj->hdr.next = env->tmp_obj_list;
env->tmp_obj_list = &obj->hdr;
obj->hdr.dtor = &readonly_check_dtor;
obj->ptr = ptr;
obj->size = sz;
obj->checksum = calc_checksum(ptr, sz);
}
void send_docking_state(uint8_t dockstate)
{
const uint8_t size = 0x05;
uint8_t msg[size];
msg[0] = IdDockingStateMessage;
msg[1] = size;
msg[2] = sequence++;
msg[3] = dockstate;
msg[4] = calc_checksum(msg);
uart0_write_buffer(msg, size);
}
void net_send_icmp(uint8_t type, uint8_t code, uint8_t *body,
uint16_t body_length) {
/**
+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
| Type | Code | Checksum |
+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
| |
+ Message Body +
| |
*/
uint8_t buf[2];
CHECK_SP("net_send_icmp: ");
buf[0] = type;
buf[1] = code;
net_send_data(buf, 2);
calc_checksum(buf, 2);
net_send_dummy_checksum();
net_send_data(body, body_length);
calc_checksum(body, body_length);
}
