static RAW_U32 buffer_to_msg(RAW_QUEUE_BUFFER *q_b, void *msg)
{
MSG_SIZE_TYPE head = q_b->head;
RAW_U8 *buffer = q_b->buffer;
MSG_SIZE_TYPE msgsz, actsz;
MSG_SIZE_TYPE remsz;
actsz = msgsz = *(HEADER*)&buffer[head];
q_b->frbufsz += (HEADERSZ + ROUND_BUFFER_SIZE(msgsz));
head += HEADERSZ;
if (head >= q_b->bufsz) {
head = 0;
}
if ((remsz = q_b->bufsz - head) < msgsz) {
raw_memcpy(msg, &buffer[head], remsz);
msg = (RAW_U8 *)msg + remsz;
msgsz -= remsz;
head = 0;
}
raw_memcpy(msg, &buffer[head], msgsz);
head += ROUND_BUFFER_SIZE(msgsz);
if (head >= q_b->bufsz) {
head = 0;
}
q_b->head = head;
return actsz;
}
static void msg_to_end_buffer(RAW_QUEUE_BUFFER *q_b, void *msg, MSG_SIZE_TYPE msgsz)
{
MSG_SIZE_TYPE tail = q_b->tail;
RAW_U8 *buffer = q_b->buffer;
MSG_SIZE_TYPE remsz;
q_b->frbufsz -= (HEADERSZ + ROUND_BUFFER_SIZE(msgsz));
*(HEADER*)&buffer[tail] = msgsz;
tail += HEADERSZ;
if (tail >= q_b->bufsz) {
tail = 0;
}
if ((remsz = q_b->bufsz - tail) < msgsz) {
raw_memcpy(&buffer[tail], msg, remsz);
msg = (RAW_U8 *)msg + remsz;
msgsz -= remsz;
tail = 0;
}
raw_memcpy(&buffer[tail], msg, msgsz);
tail += ROUND_BUFFER_SIZE(msgsz);
if (tail >= q_b->bufsz) {
tail = 0;
}
q_b->tail = tail;
}
static int
get_packet(raw_list_t *rawlist, void *buffer, int size, int offset)
{
int retval;
retval = raw_memcpy(buffer, rawlist, offset, size);
if (retval != -1) {
return (0);
}
return (-1);
}
/*
* Memory allocation size change
*/
static void *_mem_realloc( void *ptr, RAW_U32 size, MACB *macb )
{
LIST *aq;
RAW_U32 oldsz, sz;
if ( macb->testmode > 0 ) {
if ( !chkalloc(ptr, 0, macb) ) {
return 0;
}
}
/* If smaller than minimum fragment size,
allocate minimum fragment size */
if ( size > 0 && size < MIN_FRAGMENT ) {
size = MIN_FRAGMENT;
}
size = ROUND(size);
aq = (LIST *)ptr - 1;
if ( ptr != 0 ) {
/* Current allocation size */
oldsz = (RAW_U32)AreaSize(aq);
/* Merge if next space is free space */
if ( !chkAreaFlag(aq->next, AREA_END|AREA_USE) ) {
removeFreeQue(aq->next + 1);
removeAreaQue(aq->next);
}
sz = (RAW_U32)AreaSize(aq);
} else {
sz = oldsz = 0;
}
if ( size <= sz ) {
if ( size > 0 ) {
/* Fragment current area and allocate */
allocate(aq, size, macb);
} else {
/* Release area */
_mem_free(ptr, macb);
ptr = 0;
}
} else {
/* Allocate new area */
void *newptr = _mem_malloc(size, macb);
if ( newptr == 0 ) {
/* Reallocate original area at original size */
if ( ptr != 0 ) {
allocate(aq, oldsz, macb);
}
return 0;
}
if ( ptr != 0 ) {
/* Copy contents */
raw_memcpy(newptr, ptr, oldsz);
/* Release old area */
_mem_free(ptr, macb);
}
ptr = newptr;
}
return ptr;
}
RAW_U16 queue_buffer_post(RAW_QUEUE_BUFFER *q_b, RAW_VOID *p_void, MSG_SIZE_TYPE msg_size, RAW_U8 opt_send_method)
{
LIST *block_list_head;
RAW_TASK_OBJ *task_ptr;
RAW_SR_ALLOC();
RAW_CRITICAL_ENTER();
if (q_b->common_block_obj.object_type != RAW_QUEUE_BUFFER_OBJ_TYPE) {
RAW_CRITICAL_EXIT();
return RAW_ERROR_OBJECT_TYPE;
}
block_list_head = &q_b->common_block_obj.block_list;
if (!is_queue_buffer_free(q_b, msg_size)) {
RAW_CRITICAL_EXIT();
TRACE_QUEUE_BUFFER_MAX(raw_task_active, q_b, p_void, msg_size, opt_send_method);
return RAW_QUEUE_BUFFER_FULL;
}
/*Queue buffer is not full here, if there is no blocked receive task*/
if (is_list_empty(block_list_head)) {
if (opt_send_method == SEND_TO_END) {
msg_to_end_buffer(q_b, p_void, msg_size);
}
else {
}
RAW_CRITICAL_EXIT();
TRACE_QUEUE_BUFFER_POST(raw_task_active, q_b, p_void, msg_size, opt_send_method);
return RAW_SUCCESS;
}
task_ptr = list_entry(block_list_head->next, RAW_TASK_OBJ, task_list);
raw_memcpy(task_ptr->msg, p_void, msg_size);
task_ptr->qb_msg_size = msg_size;
raw_wake_object(task_ptr);
RAW_CRITICAL_EXIT();
TRACE_QUEUE_BUFFER_WAKE_TASK(raw_task_active, list_entry(block_list_head->next, RAW_TASK_OBJ, task_list), p_void, msg_size, opt_send_method);
raw_sched();
return RAW_SUCCESS;
}
static int
get_packets(hash_obj_t *seg_hash, raw_list_t *rawlist, int offset, int length)
{
int tag_size;
int paylen;
int retval;
int seg_limit = 0;
int pktcnt = 0;
char *data;
uint32_t crc;
uint32_t origcrc;
fru_tag_t tag;
hash_obj_t *pkt_hash_obj;
hash_obj_t *sec_hash;
fru_segdesc_t *segdesc;
fru_tagtype_t tagtype;
char *ignore_flag;
retval = get_packet(rawlist, &tag, sizeof (fru_tag_t), offset);
if (retval == -1) {
return (-1);
}
/* section hash object */
sec_hash = lookup_handle_object(seg_hash->u.seg_obj->section_hdl,
SECTION_TYPE);
if (sec_hash == NULL) {
return (-1);
}
seg_hash->u.seg_obj->trailer_offset = offset;
data = (char *)&tag;
while (data[0] != SEG_TRAILER_TAG) {
tagtype = get_tag_type(&tag); /* verify tag type */
if (tagtype == -1) {
return (-1);
}
tag_size = get_tag_size(tagtype);
if (tag_size == -1) {
return (-1);
}
seg_limit += tag_size;
if (seg_limit > length) {
return (-1);
}
paylen = get_payload_length((void *)&tag);
if (paylen == -1) {
return (-1);
}
seg_limit += paylen;
if (seg_limit > length) {
return (-1);
}
if ((offset + tag_size + paylen) >
(sec_hash->u.sec_obj->section.offset +
sec_hash->u.sec_obj->section.length)) {
return (-1);
}
pkt_hash_obj = create_packet_hash_object();
if (pkt_hash_obj == NULL) {
return (-1);
}
pkt_hash_obj->u.pkt_obj->payload = malloc(paylen);
if (pkt_hash_obj->u.pkt_obj->payload == NULL) {
free(pkt_hash_obj);
return (-1);
}
offset += tag_size;
retval = raw_memcpy(pkt_hash_obj->u.pkt_obj->payload, rawlist,
offset, paylen);
if (retval != paylen) {
free(pkt_hash_obj->u.pkt_obj->payload);
free(pkt_hash_obj);
return (-1);
}
/* don't change this */
pkt_hash_obj->u.pkt_obj->tag.raw_data = 0;
(void) memcpy(&pkt_hash_obj->u.pkt_obj->tag, &tag, tag_size);
pkt_hash_obj->u.pkt_obj->paylen = paylen;
pkt_hash_obj->u.pkt_obj->tag_size = tag_size;
pkt_hash_obj->u.pkt_obj->payload_offset = offset;
offset += paylen;
add_hashobject_to_hashtable(pkt_hash_obj);
add_to_pkt_object_list(seg_hash, pkt_hash_obj);
pktcnt++;
retval = get_packet(rawlist, &tag, sizeof (fru_tag_t),
offset);
if (retval == -1) {
return (retval);
}
data = (char *)&tag;
}
segdesc = (fru_segdesc_t *)&seg_hash->u.seg_obj->segment.descriptor;
seg_hash->u.seg_obj->trailer_offset = offset;
if (!segdesc->field.ignore_checksum) {
crc = get_checksum_crc(seg_hash, seg_limit);
offset = seg_hash->u.seg_obj->segment.offset;
retval = raw_memcpy(&origcrc, rawlist, offset + seg_limit + 1,
sizeof (origcrc));
ignore_flag = getenv(IGNORE_CHECK);
if (ignore_flag != NULL) {
return (pktcnt);
}
if (retval != sizeof (origcrc)) {
return (-1);
}
origcrc = BE_32(origcrc);
if (origcrc != crc) {
seg_hash->u.seg_obj->trailer_offset = offset;
return (-1);
}
}
return (pktcnt);
}
static int
get_section(raw_list_t *rawlist, hash_obj_t *sec_hash, section_t *section)
{
int retval;
int size;
int count;
uint16_t hdrver;
hash_obj_t *seg_hash;
unsigned char *buffer;
section_obj_t *sec_obj;
section_layout_t sec_hdr;
segment_layout_t *seg_hdr;
segment_layout_t *seg_buf;
sec_obj = sec_hash->u.sec_obj;
if (sec_obj == NULL) {
return (-1);
}
/* populate section_t */
section->handle = sec_hash->obj_hdl;
section->offset = sec_obj->section.offset;
section->length = sec_obj->section.length;
section->protection = sec_obj->section.protection;
section->version = sec_obj->section.version;
/* read section header layout */
retval = raw_memcpy(&sec_hdr, rawlist, sec_obj->section.offset,
sizeof (sec_hdr));
if (retval != sizeof (sec_hdr)) {
return (-1);
}
hdrver = GET_SECTION_HDR_VERSION;
if ((sec_hdr.headertag != SECTION_HDR_TAG) &&
(hdrver != section->version)) {
return (-1);
}
/* size = section layout + total sizeof segment header */
size = sizeof (sec_hdr) + ((sec_hdr.segmentcount)
* sizeof (segment_layout_t));
buffer = alloca(size);
if (buffer == NULL) {
return (-1);
}
/* segment header buffer */
seg_buf = alloca(size - sizeof (sec_hdr));
if (seg_buf == NULL) {
return (-1);
}
/* read segment header */
retval = raw_memcpy(seg_buf, rawlist,
sec_obj->section.offset + sizeof (sec_hdr),
size - sizeof (sec_hdr));
if (retval != (size - sizeof (sec_hdr))) {
return (-1);
}
/* copy section header layout */
(void) memcpy(buffer, &sec_hdr, sizeof (sec_hdr));
/* copy segment header layout */
(void) memcpy(buffer + sizeof (sec_hdr), seg_buf, size -
sizeof (sec_hdr));
/* verify crc8 */
retval = verify_header_crc8(hdrver, buffer, size);
if (retval != TRUE) {
return (-1);
}
section->version = hdrver;
sec_obj->section.version = hdrver;
seg_hdr = (segment_layout_t *)seg_buf;
/* bug fix for frutool */
if (sec_hash->u.sec_obj->seg_obj_list != NULL) {
return (0);
} else {
sec_obj->num_of_segment = 0;
}
for (count = 0; count < sec_hdr.segmentcount; count++, seg_hdr++) {
seg_hash = create_segment_hash_object();
if (seg_hash == NULL) {
return (-1);
}
add_hashobject_to_hashtable(seg_hash);
copy_segment_layout(&seg_hash->u.seg_obj->segment, seg_hdr);
add_to_seg_object_list(sec_hash, seg_hash);
sec_obj->num_of_segment++;
}
return (0);
}
