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 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);
}

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));
}

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;
}

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 headphone_pa_gain_store(struct kobject *kobj,
		struct kobj_attribute *attr, const char *buf, size_t count)
{
	unsigned int lval, rval, chksum;
	unsigned int gain, status;
	unsigned int out;

	sscanf(buf, "%u %u %u", &lval, &rval, &chksum);

	if (calc_checksum(lval, rval, chksum)) {
	gain = taiko_read(fauxsound_codec_ptr, TAIKO_A_RX_HPH_L_GAIN);
	out = (gain & 0xf0) | lval;
	taiko_write(fauxsound_codec_ptr, TAIKO_A_RX_HPH_L_GAIN, out);

	status = taiko_read(fauxsound_codec_ptr, TAIKO_A_RX_HPH_L_STATUS);
	out = (status & 0x0f) | (lval << 4);
	taiko_write(fauxsound_codec_ptr, TAIKO_A_RX_HPH_L_STATUS, out);

	gain = taiko_read(fauxsound_codec_ptr, TAIKO_A_RX_HPH_R_GAIN);
	out = (gain & 0xf0) | rval;
	taiko_write(fauxsound_codec_ptr, TAIKO_A_RX_HPH_R_GAIN, out);

	status = taiko_read(fauxsound_codec_ptr, TAIKO_A_RX_HPH_R_STATUS);
	out = (status & 0x0f) | (rval << 4);
	taiko_write(fauxsound_codec_ptr, TAIKO_A_RX_HPH_R_STATUS, out);
	}
	return count;
}

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 *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);
}

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;
		}
	}
}

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);
}

/*
 * 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);
}