static void q_ap_read(struct txn *t, u32 addr, u32 *value) {
q_check(t, 4);
q_ap_select(t, addr);
t->tx[t->txc++] = SWD_RX(OP_AP | (addr & 0xC), 1);
t->tx[t->txc++] = SWD_RD(DP_BUFFER, 1);
t->rx[t->rxc++] = value;
}
/*===========================================================================
FUNCTION Q_LINEAR_SEARCH
DESCRIPTION
Given a comparison function, this function traverses the elements in a
queue, calls the compare function on each item, and returns a pointer to
the current element's link if the user passed compare function returns
non-zero on that element.
The user compare function should return 0 if the current element is not
the element in which the compare function is interested.
The user compare function SHOULD NOT dequeue a node.
DEPENDENCIES
The specified queue should have been initialized previously via a call to
q_init.
RETURN VALUE
A pointer to the found element's queue link.
SIDE EFFECTS
A pointer to a linked element's link is returned without de-queuing the
item.
===========================================================================*/
void *q_linear_search
(
q_type *q_ptr, /* Queue to be traversed */
q_compare_func_type compare_func, /* Compare function to be called */
void *compare_val /* Cookie to pass to compare_func */
)
{
q_link_type *link_ptr = NULL; /* Link to be returned */
/*- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
/*-------------------------------------------------------------------------
Verify parameters.
-------------------------------------------------------------------------*/
ASSERT( NULL != q_ptr );
ASSERT( NULL != compare_func );
/*-------------------------------------------------------------------------
Check that the queue is initialized. Works if
FEATURE_QUEUE_NO_STRICT_CHECK is turned off.
-------------------------------------------------------------------------*/
QUEUE_CHECK_INIT( q_ptr );
link_ptr = q_check( q_ptr );
while( NULL != link_ptr )
{
if( 0 != compare_func( link_ptr, compare_val ) )
{
break;
}
link_ptr = q_next( q_ptr, link_ptr );
} /* while */
return link_ptr;
} /* q_linear_search() */
/*===========================================================================
FUNCTION Q_LINEAR_SEARCH
DESCRIPTION
Given a comparison function, this function traverses the elements in
a queue, calls the compare function, and returns a pointer to the
current element being compared if the user passed compare function
returns non zero.
The user compare function should return 0 if the current element is
not the element in which the compare function is interested.
DEPENDENCIES
The specified queue should have been initialized previously via a call
to q_init.
The user's queue elements must have q_link_type as the first element
of the queued structure.
RETURN VALUE
A pointer to the found element
SIDE EFFECTS
None.
===========================================================================*/
void* q_linear_search(
q_type *q_ptr,
q_compare_func_type compare_func,
void *compare_val
)
{
q_generic_item_type *item_ptr = NULL;
item_ptr = (q_generic_item_type*)q_check( q_ptr );
while( item_ptr != NULL )
{
if( compare_func( item_ptr, compare_val ) != 0 )
{
return item_ptr;
}
item_ptr = (q_generic_item_type*)q_next( q_ptr, &item_ptr->link );
} /* END while traversing the queue */
return NULL;
} /* END q_linear_search */
/*===========================================================================
FUNCTION DSSICMP_READ()
DESCRIPTION
Reads 'nbytes' bytes into the buffer from the ICMP transport. Fills in
address structure with values from who sent the data in fromaddr.
This function asserts that fromaddr is not NULL. This function is the
ICMP specific dss_recvfrom()
DEPENDENCIES
None.
PARAMETERS
struct scb_s* scb_ptr - Ptr to socket control block for the socket
void *buffer - user buffer from which to copy the data
uint16 nbytes - number of bytes app wants to read
struct sockaddr_in *fromaddr - source address
sint15 *dss_errno - error condition value
RETURN VALUE
n - the number of bytes to be written, which could be less than the number
of bytes specified.
On error, return DSS_ERROR and places the error condition value in
*dss_errno.
Errno Values
------------
DS_EWOULDBLOCK operation would block
SIDE EFFECTS
None.
===========================================================================*/
extern sint15 dssicmp_read
(
struct scb_s* scb_ptr, /* Ptr to socket control block for the socket */
struct iovec * iov, /* user buffer from which to copy the data */
uint16 iovcount, /* length of the iovec array */
struct sockaddr_in *fromaddr, /* source address */
sint15 *dss_errno /* error condition value */
)
{
struct icmp_cb *icmp_cb_ptr; /* ICMP protocol control block */
dsm_item_type *item_ptr; /* ptr to head of dsm memory pool items */
uint16 cnt=0; /* tmp # of bytes retrieved */
uint16 bytes_read =0; /* # of bytes to read from rcvq */
uint16 read_cnt=0; /* # of bytes read in each iteration */
uint16 payload_len=0; /* packet length */
int i; /* local loop index */
uint16 bytes_requested; /* # of bytes requested in each iteration */
/*- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -*/
MSG_LOW("In dssicmp_read()", 0, 0, 0);
/*-------------------------------------------------------------------------
Ensure that the "from address" is not NULL. This function fills in this
structure, thus, needs to ASSERT against dereferencing a NULL pointer.
------------------------------------------------------------------------*/
ASSERT( fromaddr != NULL);
/*-------------------------------------------------------------------------
Set the socket and ICMP control block pointers, and set the family to
AF_INET.
-------------------------------------------------------------------------*/
icmp_cb_ptr = scb_ptr->protocol_ctl_blk.icb;
fromaddr->sin_family = AF_INET;
MSG_LOW("ICMP CB pointer successfully set", 0, 0, 0);
/*-------------------------------------------------------------------------
Check if there is anything in the ICMP receive queue. If not, return
DS_EWOULDBLOCK.
-------------------------------------------------------------------------*/
if ( (item_ptr = (dsm_item_type *) q_get( (&(icmp_cb_ptr->rcvq)))) == NULL)
{
MSG_LOW("Nothing on ICMP revq", 0, 0, 0);
*dss_errno = DS_EWOULDBLOCK;
return (DSS_ERROR);
}
MSG_LOW("There is indeed something on the ICMP rcvq", 0, 0, 0);
/*-------------------------------------------------------------------------
Extract the payload length, server's IP address and store in fromaddr.
ASSERT that bytes to read, are equal to the number of bytes pulled up.
The port field will be undefined in the fromaddr structure returned to
the application.
-------------------------------------------------------------------------*/
cnt = dsm_pullup(&item_ptr, &(fromaddr->sin_addr),
sizeof(fromaddr->sin_addr));
ASSERT( cnt == sizeof(fromaddr->sin_addr) );
MSG_MED("extracted server's IP %x", fromaddr->sin_addr.s_addr, 0, 0);
/*-------------------------------------------------------------------------
Extract the number of bytes which the application wants to read.
-------------------------------------------------------------------------*/
payload_len = dsm_length_packet( item_ptr );
for(i=0;i< iovcount && payload_len > 0 ; i++)
{
/*-----------------------------------------------------------------------
Extract the number of bytes which the application wants to read.
-----------------------------------------------------------------------*/
bytes_requested = MIN( payload_len, iov[i].iov_len);
if(bytes_requested > 0)
{
read_cnt = dsm_pullup( &(item_ptr), iov[i].iov_base, bytes_requested);
}
/*-----------------------------------------------------------------------
ASSERT that we read the expected number of bytes from the buffer.
-----------------------------------------------------------------------*/
ASSERT(read_cnt == bytes_requested);
payload_len -= read_cnt;
bytes_read += read_cnt;
read_cnt = 0;
}
if (payload_len > 0 )
{
ERR("User provided buffer is smaller than received datagram (%d bytes)",
bytes_read + payload_len, 0, 0);
}
MSG_LOW("Successfully read nbytes of data in DSSICMP_READ", 0, 0, 0);
/*-------------------------------------------------------------------------
Free the packet in dsm buffer.
-------------------------------------------------------------------------*/
dsm_free_packet ( &item_ptr );
MSG_LOW("Packet is successfully freed in DSSICMP_READ", 0, 0, 0);
/*-------------------------------------------------------------------------
Check if there are any remaining ICMP packets in the receive queue. Set
the readable flag to FALSE, if there are no remaining ICMP packets.
Access to global SCB array item is protected through
INTLOCK()/INTFREE().
-------------------------------------------------------------------------*/
if ( (item_ptr = (dsm_item_type *) q_check( &(icmp_cb_ptr->rcvq))) == NULL)
{
INTLOCK();
scb_ptr->data_available = FALSE;
INTFREE();
}
/*-------------------------------------------------------------------------
Convert the server IP address into the Network byte order.
Note, only the IP address is in host order, the port number is not.
-------------------------------------------------------------------------*/
(fromaddr->sin_addr).s_addr = dss_htonl( (fromaddr->sin_addr).s_addr);
/*-------------------------------------------------------------------------
Return the number of bytes read from the buffer.
-------------------------------------------------------------------------*/
MSG_LOW("DONE ICMPREAD", 0, 0, 0);
return ( (sint15) bytes_read);
} /* dssudp_read() */
static void q_ap_write(struct txn *t, u32 addr, u32 value) {
q_check(t, 3);
q_ap_select(t, addr);
t->tx[t->txc++] = SWD_WR(OP_AP | (addr & 0xC), 1);
t->tx[t->txc++] = value;
}
/*===========================================================================
FUNCTION Q_LINEAR_DELETE
DESCRIPTION
Given a comparison function, this function traverses the elements in
a queue, calls the compare function, and returns a pointer to the
current element being compared if the user passed compare function
returns non zero. In addition, the item will be removed from the queue.
The user compare function should return 0 if the current element is
not the element in which the compare function is interested.
DEPENDENCIES
The specified queue should have been initialized previously via a call
to q_init.
The user's queue elements must have q_link_type as the first element
of the queued structure.
The user's compare function will be passed NULL for the compare value.
RETURN VALUE
None
SIDE EFFECTS
None.
===========================================================================*/
void q_linear_delete(
q_type *q_ptr,
q_compare_func_type compare_func,
void *param,
q_action_func_type action_func
)
{
q_generic_item_type *item_ptr = NULL;
/* Used in the traversal to point to the current item
*/
q_generic_item_type *prev_ptr = NULL;
/* Used in the traversal to point to the item previous to
** the current item. This makes removing the current item
** a constant time operation
*/
/* User must provide a compare function, otherwise, this is
** meaningless.
*/
if( compare_func == NULL )
{
return;
}
q_lock( q_ptr );
/* item_ptr points to the first item on the list
*/
item_ptr = (q_generic_item_type*)q_check( q_ptr );
prev_ptr = NULL;
while( item_ptr != NULL )
{
if( compare_func( item_ptr, NULL ) != 0 )
{
/* Remove the item
*/
if( prev_ptr != NULL )
{
/* Remove from the middle or tail
*/
prev_ptr->link.next_ptr = item_ptr->link.next_ptr;
item_ptr->link.next_ptr = NULL;
}
else
{
/* Remove from the head
*/
q_get( q_ptr );
}
/* Call the action function if there is one
*/
if( action_func )
{
action_func( item_ptr, param );
}
break;
}
/* Move on to the next item
*/
prev_ptr = item_ptr;
item_ptr = (q_generic_item_type*)q_next( q_ptr, &item_ptr->link );
} /* END while traversing the queue */
q_free( q_ptr );
return;
} /* END q_linear_delete */
