static char *encodeAtrChangeRequest(char *ptr, const StunAtrChangeRequest *atr)
{
ptr = encode16(ptr, ChangeRequest);
ptr = encode16(ptr, 4);
ptr = encode32(ptr, atr->value);
return ptr;
}
static char *encodeAtrAddress4(char *ptr, uint16_t type,
const StunAtrAddress4 *atr)
{
ptr = encode16(ptr, type);
ptr = encode16(ptr, 8);
*ptr++ = atr->pad;
*ptr++ = IPv4Family;
ptr = encode16(ptr, atr->ipv4.port);
ptr = encode32(ptr, atr->ipv4.addr);
return ptr;
}
/* common 64 byte block padding and count code for SHA2_224 and SHA2_256 */
static DigestState*
sha2_64(uchar *p, ulong len, uchar *digest, SHA2_256state *s, int dlen)
{
int i;
u32int x[16];
uchar buf[128];
uchar *e;
/* fill out the partial 64 byte block from previous calls */
if(s->blen){
i = 64 - s->blen;
if(len < i)
i = len;
memmove(s->buf + s->blen, p, i);
len -= i;
s->blen += i;
p += i;
if(s->blen == 64){
_sha2block64(s->buf, s->blen, s->state);
s->len += s->blen;
s->blen = 0;
}
}
/* do 64 byte blocks */
i = len & ~(64-1);
if(i){
_sha2block64(p, i, s->state);
s->len += i;
len -= i;
p += i;
}
/* save the left overs if not last call */
if(digest == 0){
if(len){
memmove(s->buf, p, len);
s->blen += len;
}
return s;
}
/*
* this is the last time through, pad what's left with 0x80,
* 0's, and the input count to create a multiple of 64 bytes.
*/
if(s->blen){
p = s->buf;
len = s->blen;
} else {
memmove(buf, p, len);
p = buf;
}
s->len += len;
e = p + len;
if(len < 56)
i = 56 - len;
else
i = 120 - len;
memset(e, 0, i);
*e = 0x80;
len += i;
/* append the count */
x[0] = s->len>>29;
x[1] = s->len<<3;
encode32(p+len, x, 8);
/* digest the last part */
_sha2block64(p, len+8, s->state);
s->len += len+8;
/* return result and free state */
encode32(digest, s->state, dlen);
if(s->malloced == 1)
free(s);
return nil;
}
H_U32 _StorageUpdata(device_storage_gather_t *device_info)
{
H_U32 retcode;
WYPrintf(MODULE_DEBUG_STORAGE,LEVEL_DEBUG,"in __storage_device_updata");
CHECK_PARAMETER(device_info, H_NULL, H_FAULTER);
//H_U8 * buffer = H_NULL;
//buffer = (H_U8 *)wy_malloc(STORAGE_BLOCK_LEN);
//if( buffer == H_NULL)
//{
// WYPrintf(MODULE_DEBUG_STORAGE,LEVEL_DEBUG,"malloc fail");
// return H_FAULTER;
//}
static H_U8 storage_buffer[STORAGE_BLOCK_LEN] = {0};
H_U32 offset = 0;
H_U32 magic_data = 0;
wy_memset(storage_buffer,0xFF,STORAGE_BLOCK_LEN);
//WYPrintf(MODULE_DEBUG_STORAGE,LEVEL_DEBUG,"reading entire block...");
if(_StorageRead(device_info->handler, STORAGE_BLOCK_LEN,0,storage_buffer,device_info->user_block_size) != H_SUCCESS)
{
WYPrintf(MODULE_DEBUG_STORAGE,LEVEL_DEBUG,"read fail");
//wy_free(buffer);
//buffer = H_NULL;
return H_FAULTER;
}
//WYPrintf_array(storage_buffer, STORAGE_BLOCK_LEN);
wy_tools_op()->_delay_ms(10);
offset = device_info->id * device_info->user_block_size ;
WYPrintf(MODULE_DEBUG_STORAGE,LEVEL_DEBUG,"offset will be:%d",offset);
magic_data = decode32(storage_buffer, offset);
if(magic_data == device_info->magic_data || magic_data == 0) // check whether this block has been written before and is written by correct module
{ //workaround
WYPrintf(MODULE_DEBUG_STORAGE,LEVEL_DEBUG,"magic1");
if(device_info->magic_clear)//delete a record
{
WYPrintf(MODULE_DEBUG_STORAGE,LEVEL_DEBUG,"magic2");
wy_memset(&(storage_buffer[offset]), 0xff, device_info->hand_len);
}
wy_memcpy(&(storage_buffer[offset + device_info->hand_len]), device_info->data, device_info->data_len);
}
else //this block has not been written before
{
if(!device_info->magic_clear)//add a record
{
encode32(device_info->magic_data, storage_buffer, offset);
wy_memcpy(&(storage_buffer[offset + device_info->hand_len]), device_info->data, device_info->data_len);
WYPrintf(MODULE_DEBUG_STORAGE,LEVEL_DEBUG,"magic3:%x ",device_info->magic_data);
}
}
retcode = _StorageErase(device_info->handler,STORAGE_BLOCK_LEN);
WYPrintf(MODULE_DEBUG_STORAGE,LEVEL_DEBUG,"erasing module:%d block :%d",device_info->handler->module_id,device_info->handler->block_id);
WYPrintf(MODULE_DEBUG_STORAGE,LEVEL_DEBUG,"ret of erase:%d",retcode);
if(retcode != H_SUCCESS)
{
WYPrintf(MODULE_DEBUG_STORAGE,LEVEL_DEBUG,"erase fail");
//wy_free(buffer);
//buffer = H_NULL;
return H_FAULTER;
}
//wy_tools_op()->_delay_ms(100);
//WYPrintf(MODULE_DEBUG_STORAGE,LEVEL_DEBUG,"before write1 for module:%d",device_info->handler->module_id);
retcode = _StorageWrite(device_info->handler,storage_buffer,STORAGE_BLOCK_LEN, 0,device_info->user_block_size);
WYPrintf(MODULE_DEBUG_STORAGE,LEVEL_DEBUG,"ret of write:%d block size:%d",retcode,device_info->user_block_size);
if(retcode != H_SUCCESS)
{
WYPrintf(MODULE_DEBUG_STORAGE,LEVEL_DEBUG,"rewrite fail");
//wy_free(buffer);
//buffer = H_NULL;
return H_FAULTER;
}
wy_tools_op()->_delay_ms(100);
//wy_free(buffer);
//buffer = H_NULL;
return H_SUCCESS;
}
