inline void print_tx_buffer()
{
int8_t i;
nrk_kprintf(PSTR("Transmit Buffer: "));
if(tx_buf_mgr.head_fq == NULL)
printf("-1 ");
else
printf("%d ", convert_ptr_to_index(tx_buf_mgr.head_fq));
if(tx_buf_mgr.tail_fq == NULL)
printf("-1 ");
else
printf("%d ", convert_ptr_to_index(tx_buf_mgr.tail_fq));
if(tx_buf_mgr.head_aq == NULL)
printf("-1 ");
else
printf("%d ", convert_ptr_to_index(tx_buf_mgr.head_aq));
if(tx_buf_mgr.tail_aq == NULL)
printf("-1\n");
else
printf("%d\n", convert_ptr_to_index(tx_buf_mgr.tail_aq));
for(i = 0; i < MAX_TX_QUEUE_SIZE; i++)
print_pkt(&(tx_buf[i].pkt));
return;
}
void
print_capture (const struct pcap_metadata *metadata, const char *pkt,
unsigned int len, int verbose)
{
struct timeval now;
struct tm *loctime;
char bufftime[BUFSIZ];
gettimeofday (&now, NULL);
loctime = gmtime (&(now.tv_sec));
strftime (bufftime, BUFSIZ, "%H:%M:%S", loctime);
if (verbose == JSON_OUTPUT) {
printf ("{\"timestamp\": \"%s.%lu\", ", bufftime, now.tv_usec);
} else {
printf ("%s.%lu ", bufftime, now.tv_usec);
}
print_metadata (metadata, verbose);
print_pkt (pkt, len, verbose);
if (verbose == JSON_OUTPUT) {
printf("}\n");
}
}
static int cb(struct nfq_q_handle *qh, struct nfgenmsg *nfmsg,
struct nfq_data *nfa, void *data)
{
u_int32_t id = print_pkt(nfa);
printf("entering callback\n");
return nfq_set_verdict(qh, id, NF_ACCEPT, 0, NULL);
}
static int cb(struct nfq_q_handle *qh, struct nfgenmsg *nfmsg,
struct nfq_data *nfa, void *data)
{
u_int32_t id = print_pkt(nfa);
if(use_pcap == 1)
record_pkt(nfa);
return nfq_set_verdict(qh, id, NF_ACCEPT, 0, NULL);
}
static void nic_pkt_received(struct nic * nic, unsigned size)
{
assert(nic->netif.input);
#if 0
print_pkt((unsigned char *) nic->rx_pbuf->payload, 64 /*nic->rx_pbuf->len */);
#endif
assert(nic->rx_pbuf->tot_len == nic->rx_pbuf->len);
nic->rx_pbuf->tot_len = nic->rx_pbuf->len = size - ETH_CRC_SIZE;
nic->netif.input(nic->rx_pbuf, &nic->netif);
nic->rx_pbuf = NULL;
driver_setup_read(nic);
}
/* int eth_send
* ( eth_iface_t * iface,
* mac_addr_t dst, uint16_t type, unsigned char * data, int data_len );
*
* DESCRIPCIÓN:
* Esta función permite enviar una trama Ethernet a través de la interfaz
* indicada.
*
* PARÁMETROS:
* 'iface': Manejador de la interfaz Ethernet por la que se quiere enviar
* el paquete.
* La interfaz debe haber sido inicializada con 'eth_open()'
* previamente.
* 'dst': Dirección MAC del equipo destino.
* 'type': Valor del campo 'Tipo' de la trama Ethernet a enviar.
* 'data': Puntero a los datos que se quieren enviar en la trama
* Ethernet.
* 'data_len': Longitud en bytes de los datos a enviar.
*
* VALOR DEVUELTO:
* El número de bytes que han podido ser enviados.
*
* ERRORES:
* La función devuelve '-1' si se ha producido algún error.
*/
int eth_send(eth_iface_t * iface, mac_addr_t dst, uint16_t type, unsigned char * payload, int payload_len) {
int bytes_sent;
/* Comprobar parámetros */
if (iface == NULL) {
fprintf(stderr, "eth_send(): ERROR: iface == NULL\n");
return -1;
}
/* Crear la Trama Ethernet y rellenar todos los campos */
struct eth_frame eth_frame;
memcpy(eth_frame.dest_addr, dst, MAC_ADDR_SIZE);
memcpy(eth_frame.src_addr, iface->mac_address, MAC_ADDR_SIZE);
eth_frame.type = htons(type);
memcpy(eth_frame.payload, payload, payload_len);
int eth_frame_len = ETH_HEADER_SIZE + payload_len;
/* DEBUG */
if (ETH_LOG >= VERBOSE_LOG_LEVEL) {
char* iface_name = eth_getname(iface);
char mac_addr_str[MAC_ADDR_STR_LENGTH];
eth_mac_str(dst, mac_addr_str);
printf("eth_send(type=0x%04x, payload[%d]) > %s/%s\n", type, payload_len, iface_name, mac_addr_str);
if (ETH_LOG >= DEBUG_LOG_LEVEL) {
print_pkt((unsigned char *) ð_frame, eth_frame_len, ETH_HEADER_SIZE);
}
}
/* Enviar la trama Ethernet creada con rawnet_send() y comprobar errores */
bytes_sent = rawnet_send(iface->raw_iface, (unsigned char *) ð_frame, eth_frame_len);
if (bytes_sent == -1) {
fprintf(stderr, "eth_send(): ERROR en rawnet_send(): %s\n", rawnet_strerror());
return -1;
}
/* Devolver el número de bytes de datos recibidos */
return (bytes_sent - ETH_HEADER_SIZE);
}
/* int eth_recv
* ( eth_iface_t * iface,
* mac_addr_t src, uint16_t type, unsigned char buffer[], long int timeout );
*
* DESCRIPCIÓN:
* Esta función permite obtener el siguiente paquete recibido por la
* interfaz Ethernet indicada. La operación puede esperar indefinidamente o
* un tiempo limitando dependiento del parámetro 'timeout'.
*
* Esta función sólo permite recibir paquetes de una única interfaz. Si desea
* escuchar varias interfaces Ethernet simultaneamente, utilice la función
* 'eth_poll()'.
*
* PARÁMETROS:
* 'iface': Manejador de la interfaz Ethernet por la que se desea recibir
* un paquete.
* La interfaz debe haber sido inicializada con 'eth_open()'
* previamente.
* 'src': Dirección MAC del equipo que envió la trama Ethernet recibida.
* Este es un parámetro de salida. La dirección se copiará en la
* memoria indicada, que debe estar reservada previamente.
* 'type': Valor del campo 'Tipo' de la trama Ethernet que se desea
* recibir.
* Las tramas con un valor 'type' diferente serán descartadas.
* 'buffer': Array donde se almacenarán los datos de la trama recibida.
* Deben reservarse al menos 'ETH_MTU' bytes para recibir dichos
* datos.
* 'timeout': Tiempo en milisegundos que debe esperarse a recibir una trama
* antes de retornar. Un número negativo indicará que debe
* esperarse indefinidamente, mientras que con un '0' la función
* retornará inmediatamente, se haya recibido o no una trama.
*
* VALOR DEVUELTO:
* La longitud en bytes de los datos de la trama recibida, o '0' si no se ha
* recibido ninguna trama porque ha expirado el temporizador.
*
* ERRORES:
* La función devuelve '-1' si se ha producido algún error.
*/
int eth_recv(eth_iface_t * iface, mac_addr_t src, uint16_t type, unsigned char buffer[], long int timeout) {
int payload_len;
/* Comprobar parámetros */
if (iface == NULL) {
fprintf(stderr, "eth_recv(): ERROR: iface == NULL\n");
return -1;
}
/* Inicializar temporizador para mantener timeout si se reciben tramas con
tipo incorrecto. */
timerms_t timer;
timerms_reset(&timer, timeout);
int frame_len;
unsigned char eth_buffer[ETH_FRAME_MAX_SIZE];
struct eth_frame * eth_frame_ptr = NULL;
int is_target_type;
int is_my_mac;
int is_multicast;
do {
long int time_left = timerms_left(&timer);
/* Recibir trama del interfaz Ethernet y procesar errores */
frame_len = rawnet_recv(iface->raw_iface, eth_buffer, ETH_FRAME_MAX_SIZE, time_left);
if (frame_len == -1) {
fprintf(stderr, "eth_recv(): ERROR en rawnet_recv(): %s\n", rawnet_strerror());
return -1;
} else if (frame_len == 0) {
/* DEBUG */
if (ETH_LOG >= NORMAL_LOG_LEVEL) {
printf("eth_recv(): ¡ Timeout !\n");
}
return 0;
}
/* Comprobar si es la trama que estamos buscando */
eth_frame_ptr = (struct eth_frame *) eth_buffer;
is_my_mac = (memcmp(eth_frame_ptr->dest_addr, iface->mac_address, MAC_ADDR_SIZE) == 0);
is_target_type = (ntohs(eth_frame_ptr->type) == type);
/* Es el paquete multicast? (comprobamos el bit de la MAC) */
is_multicast = (eth_frame_ptr->dest_addr[0] && 0x1);
} while (!((is_my_mac || is_multicast) && is_target_type));
/* Trama recibida con 'tipo' indicado. Copiar datos y dirección MAC origen */
memcpy(src, eth_frame_ptr->src_addr, MAC_ADDR_SIZE);
payload_len = frame_len - ETH_HEADER_SIZE;
memcpy(buffer, eth_frame_ptr->payload, payload_len);
/* DEBUG */
if (ETH_LOG >= VERBOSE_LOG_LEVEL) {
char mac_addr_str[MAC_ADDR_STR_LENGTH];
eth_mac_str(src, mac_addr_str);
char* iface_name = eth_getname(iface);
printf("eth_recv(type=0x%04x, payload[%d]) < %s/%s\n", type, payload_len, iface_name, mac_addr_str);
if (ETH_LOG >= DEBUG_LOG_LEVEL) {
print_pkt((unsigned char *) eth_frame_ptr, frame_len, ETH_HEADER_SIZE);
}
}
return payload_len;
}
void nl_tx_task()
{
TransmitBuffer *ptr = NULL; // pointer to the buffer to be transmitted
nrk_sig_t tx_done_signal; // to hold the tx_done signal from the link layer
int8_t ret; // to hold the return value of various functions
int8_t port_index; // to store the index of the corresponding port element
int8_t sent; // to count the number of times the HELLO msg was sent
int8_t isApplication; // flag to indicate whether the packet in the transmit
// buffer is an APPLICATION / NW_CONTROL packet
nrk_time_t timeout;
nrk_time_t start; // used for sending network control messages
nrk_time_t end;
nrk_time_t elapsed;
// wait for the nl_rx_task to start bmac
while(!bmac_started()) nrk_wait_until_next_period();
// retrieve and register for the 'transmit done' signal from bmac
tx_done_signal = bmac_get_tx_done_signal();
if( nrk_signal_register(tx_done_signal) == NRK_ERROR )
{
nrk_int_disable();
nrk_led_set(RED_LED);
while(1)
nrk_kprintf(PSTR("NL: Error while registering for the bmax_tx_done_signal\r\n"));
}
// initialise the timer
nrk_time_get(&start);
end.secs = start.secs;
end.nano_secs = start.nano_secs;
sent = 0;
// set the radio power
if( bmac_set_rf_power(10) == NRK_ERROR)
{
nrk_led_set(RED_LED);
nrk_int_disable();
while(1)
nrk_kprintf(PSTR("Error setting the transmit power\r\n"));
}
while(1)
{
isApplication = FALSE; // assume at the beginning that a nw_ctrl pkt will be transmitted
ret = nrk_time_sub(&elapsed, end, start);
if(ret == 0)
{
nrk_int_disable();
nrk_led_set(RED_LED);
while(1)
nrk_kprintf(PSTR("NL: Error returned by nrk_time_sub\r\n"));
}
nrk_time_compact_nanos(&elapsed);
if(elapsed.secs >= HELLO_PERIOD)
{
sent++;
build_Msg_Hello(&mhe); // build the 'HELLO' message
if(DEBUG_NL == 2)
{
nrk_kprintf(PSTR("After building Msg_Hello, packet = "));
print_pkt(&pkt_tx);
}
enter_cr(bm_sem, 34);
ret = insert_tx_aq(&pkt_tx); // insert it into the transmit queue
leave_cr(bm_sem, 34);
if(DEBUG_NL == 2)
{
nrk_kprintf(PSTR("build_Msg_Hello() inserted packet."));
print_tx_buffer();
//print_pkt_header(&pkt_tx);
}
if(ret == NRK_ERROR && DEBUG_NL == 2)
{
nrk_kprintf(PSTR("HELLO msg was not inserted into the transmit queue\r\n"));
}
start.secs = end.secs;
start.nano_secs = end.nano_secs; // reinitialise the timer
}
if(sent >= 3) //NGB_LIST_PERIOD / HELLO_PERIOD) // NGB_LIST period is always a multiple of HELLO_PERIOD
{
build_Msg_NgbList(&mn); // build the 'NGB_LIST' message
enter_cr(bm_sem, 34);
ret = insert_tx_aq(&pkt_tx); // insert it into the transmit queue
leave_cr(bm_sem, 34);
if(DEBUG_NL == 2)
{
nrk_kprintf(PSTR("build_Msg_NgbList() inserted packet."));
print_tx_buffer();
//print_pkt_header(&pkt_tx);
}
if(ret == NRK_ERROR && DEBUG_NL == 2)
{
nrk_kprintf(PSTR("NGB_LIST msg was not inserted into the transmit queue\r\n"));
}
sent = 0; // reset the value of 'sent'
}
if(rand() % 2 == 0) // random number generator
collect_queue_statistics();
enter_cr(bm_sem, 34);
ptr = remove_tx_aq();
leave_cr(bm_sem, 34);
if(ptr == NULL) // transmit queue is empty
{
if(DEBUG_NL == 2)
nrk_kprintf(PSTR("NL:Transmit queue is empty\r\n"));
// update the end time
nrk_time_get(&end);
nrk_wait_until_next_period(); // FIX ME
continue;
}
if(DEBUG_NL == 2)
{
nrk_kprintf(PSTR("NL: nl_tx_task(): Packet removed. Packet = "));
//print_pkt_header( &(ptr -> pkt) );
print_pkt( &(ptr -> pkt) );
//print_tx_buffer();
}
// check to see the type of packet. It should be of type APPLICATION and be sent by this
// node
if( (pkt_type(&(ptr -> pkt)) == APPLICATION) && ((ptr -> pkt).src == NODE_ADDR) )
{
// remove the encapsulated TL segment from the packet
unpack_TL_UDP_header(&seg, (ptr -> pkt).data);
memcpy(seg.data, (ptr -> pkt).data + SIZE_TRANSPORT_UDP_HEADER, MAX_APP_PAYLOAD);
if(DEBUG_NL == 2)
{
nrk_kprintf(PSTR("NL: nl_tx_task(): Segment Removed = "));
print_seg(&seg);
}
isApplication = TRUE;
}
// pack the network packet header into the transmit buffer
pack_NW_Packet_header(tx_buf, &(ptr -> pkt));
// append the network payload into the transmit buffer
memcpy(tx_buf + SIZE_NW_PACKET_HEADER, (ptr -> pkt).data, MAX_NETWORK_PAYLOAD);
enter_cr(bm_sem, 34);
insert_tx_fq(ptr); // release the transmit buffer into the free queue
leave_cr(bm_sem, 34);
if(DEBUG_NL == 2)
{
nrk_kprintf(PSTR("NL: nl_tx_task(): Released transmit buffer back into queue\n"));
print_tx_buffer();
}
do
{
ret = bmac_tx_pkt_nonblocking(tx_buf, SIZE_NW_PACKET); // try to queue the buffer in link layer
if(ret == NRK_ERROR)
if(nrk_event_wait(SIG(tx_done_signal)) == 0)
{
nrk_int_disable();
nrk_led_set(RED_LED);
while(1)
nrk_kprintf(PSTR("NL: Error returned by nrk_event_wait(tx_done_signal)\r\n"));
}
}while(ret == NRK_ERROR);
// packet is queued at link layer
timeout.secs = 10; // set a wait period of maximum 10 seconds
timeout.nano_secs = 0;
if( nrk_signal_register(nrk_wakeup_signal) == NRK_ERROR )
{
nrk_int_disable();
nrk_led_set(RED_LED);
while(1)
nrk_kprintf(PSTR("NL:nl_tx(): Error registering for nrk_wakeup_signal\r\n"));
}
if( nrk_set_next_wakeup(timeout) == NRK_ERROR)
{
nrk_int_disable();
nrk_led_set(RED_LED);
while(1)
nrk_kprintf(PSTR("NL: nl_tx(): Error returned by nrk_set_next_wakeup()\r\n"));
}
nrk_led_set(BLUE_LED);
ret = nrk_event_wait (SIG(tx_done_signal) | SIG(nrk_wakeup_signal)); // wait for its transmission
if(ret == 0)
{
nrk_int_disable();
nrk_led_set(RED_LED);
while(1)
nrk_kprintf(PSTR("NL: Error returned by nrk_event_wait(tx_done_signal)\r\n"));
}
if(ret & SIG(tx_done_signal)) // bmac has successfully sent the packet over the radio
{
if(isApplication == TRUE) // it was an application layer packet
{
enter_cr(tl_sem, 34);
port_index = port_to_port_index(seg.srcPort);
if(port_index == NRK_ERROR) // sanity check
{
nrk_int_disable();
nrk_led_set(RED_LED);
while(1)
nrk_kprintf(PSTR("NL: nl_tx_task: Bug detected in implementation of port element array\r\n"));
}
// signal 'send done' signal
if(nrk_event_signal(ports[port_index].send_done_signal) == NRK_ERROR)
{
if(nrk_errno_get() == 1) // sanity check. This means signal was not created
{
nrk_int_disable();
nrk_led_set(RED_LED);
while(1)
nrk_kprintf(PSTR("NL: nl_tx_task: Bug detected in creating signals in port element array\r\n"));
}
}
leave_cr(tl_sem, 34);
}// end if(isApplication == TRUE)
else // a network control message was transmitted. Nothing to signal
; // do nothing
} // end if(signal received = tx_done_signal)
else if(ret & SIG(nrk_wakeup_signal))
{
//nrk_led_set(RED_LED);
//nrk_int_disable();
//while(1)
//{
nrk_kprintf(PSTR("BMAC did not transmit the packet within specified time\r\n"));
//}
}
else // unknown signal caught
{
nrk_int_disable();
nrk_led_set(RED_LED);
while(1)
nrk_kprintf(PSTR("NL: nl_tx_task(): Unknown signal caught\r\n"));
}
nrk_led_clr(BLUE_LED);
// update the end time
nrk_time_get(&end);
} // end while(1)
return;
}
void
client_nat_transform(const struct client_nat_option_list *list,
struct buffer *ipbuf,
const int direction)
{
struct ip_tcp_udp_hdr *h = (struct ip_tcp_udp_hdr *) BPTR(ipbuf);
int i;
uint32_t addr, *addr_ptr;
const uint32_t *from, *to;
int accumulate = 0;
unsigned int amask;
unsigned int alog = 0;
if (check_debug_level(D_CLIENT_NAT))
{
print_pkt(&h->ip, "BEFORE", direction, D_CLIENT_NAT);
}
for (i = 0; i < list->n; ++i)
{
const struct client_nat_entry *e = &list->entries[i]; /* current NAT rule */
if (e->type ^ direction)
{
addr = *(addr_ptr = &h->ip.daddr);
amask = 2;
}
else
{
addr = *(addr_ptr = &h->ip.saddr);
amask = 1;
}
if (direction)
{
from = &e->foreign_network;
to = &e->network;
}
else
{
from = &e->network;
to = &e->foreign_network;
}
if (((addr & e->netmask) == *from) && !(amask & alog))
{
/* pre-adjust IP checksum */
ADD_CHECKSUM_32(accumulate, addr);
/* do NAT transform */
addr = (addr & ~e->netmask) | *to;
/* post-adjust IP checksum */
SUB_CHECKSUM_32(accumulate, addr);
/* write the modified address to packet */
*addr_ptr = addr;
/* mark as modified */
alog |= amask;
}
}
if (alog)
{
if (check_debug_level(D_CLIENT_NAT))
{
print_pkt(&h->ip, "AFTER", direction, D_CLIENT_NAT);
}
ADJUST_CHECKSUM(accumulate, h->ip.check);
if (h->ip.protocol == OPENVPN_IPPROTO_TCP)
{
if (BLEN(ipbuf) >= sizeof(struct openvpn_iphdr) + sizeof(struct openvpn_tcphdr))
{
ADJUST_CHECKSUM(accumulate, h->u.tcp.check);
}
}
else if (h->ip.protocol == OPENVPN_IPPROTO_UDP)
{
if (BLEN(ipbuf) >= sizeof(struct openvpn_iphdr) + sizeof(struct openvpn_udphdr))
{
ADJUST_CHECKSUM(accumulate, h->u.udp.check);
}
}
}
}
// Handles the incomming CAN Packets
// Each ID that deals with specific controllers a note is
// given where that info came from. There could be a lot of overlap
// and exceptions here. -- Craig
void handle_pkt(int can, struct canfd_frame frame) {
if(DEBUG) print_pkt(frame);
switch(frame.can_id) {
case 0x243: // EBCM / GM / Chevy Malibu 2006
switch(frame.data[1]) {
case UDS_SID_TESTER_PRESENT:
if(verbose > 1) plog("Received TesterPresent\n");
generic_OK_resp_to(can, frame, 0x643);
break;
case UDS_SID_GM_READ_DIAG_INFO:
handle_gm_read_diag(can, frame);
break;
default:
if(verbose) print_pkt(frame);
if(verbose) plog("Unhandled mode/sid: %s\n", get_mode_str(frame));
break;
}
break;
case 0x244: // Body Control Module / GM / Chevy Malibu 2006
if(frame.data[0] == 0x30) { // Flow control
flow_control_push_to(can, 0x644);
return;
}
switch(frame.data[1]) {
case UDS_SID_TESTER_PRESENT:
if(verbose > 1) plog("Received TesterPresent\n");
generic_OK_resp_to(can, frame, 0x644);
break;
case UDS_SID_GM_READ_DIAG_INFO:
handle_gm_read_diag(can, frame);
break;
case UDS_SID_GM_READ_DATA_BY_ID:
handle_gm_read_data_by_id(can, frame);
break;
case UDS_SID_GM_READ_DID_BY_ID:
handle_gm_read_did_by_id(can, frame);
break;
default:
if(verbose) print_pkt(frame);
if(verbose) plog("Unhandled mode/sid: %s\n", get_mode_str(frame));
break;
}
break;
case 0x24A: // Power Steering / GM / Chevy Malibu 2006
switch(frame.data[1]) {
default:
if(verbose) print_pkt(frame);
if(verbose) plog("Unhandled mode/sid: %s\n", get_mode_str(frame));
break;
}
break;
case 0x350: // Unsure. Seen RTRs to this when requesting VIN
if (frame.can_id & CAN_RTR_FLAG) {
if (verbose) plog("Received a RTR at ID %02X\n", frame.can_id);
}
break;
case 0x710: // VCDS
if(verbose) print_pkt(frame);
handle_vcds_710(can, frame);
break;
case 0x7df:
case 0x7e0: // Sometimes flow control comes here
if(verbose) print_pkt(frame);
if(frame.data[0] == 0x30 && gBufLengthRemaining > 0) flow_control_push(can);
if(frame.data[0] == 0 || frame.len == 0) return;
if(frame.data[0] > frame.len) return;
switch (frame.data[1]) {
case OBD_MODE_SHOW_CURRENT_DATA:
handle_current_data(can, frame);
break;
case OBD_MODE_SHOW_FREEZE_FRAME:
handle_freeze_frame(can, frame);
break;
case OBD_MODE_READ_DTC:
handle_stored_codes(can, frame);
break;
case OBD_MODE_READ_PENDING_DTC:
handle_pending_codes(can, frame);
break;
case OBD_MODE_VEHICLE_INFORMATION:
handle_vehicle_info(can, frame);
break;
case OBD_MODE_READ_PERM_DTC:
handle_perm_codes(can, frame);
break;
case UDS_SID_DIAGNOSTIC_CONTROL: // DSC
handle_dsc(can, frame);
break;
case UDS_SID_READ_DATA_BY_ID:
handle_read_data_by_id(can, frame);
break;
case UDS_SID_TESTER_PRESENT:
if(verbose > 1) plog("Received TesterPresent\n");
generic_OK_resp(can, frame);
break;
case UDS_SID_GM_READ_DIAG_INFO:
handle_gm_read_diag(can, frame);
break;
default:
//if(verbose) plog("Unhandled mode/sid: %02X\n", frame.data[1]);
if(verbose) plog("Unhandled mode/sid: %s\n", get_mode_str(frame));
break;
}
break;
default:
if (DEBUG) print_pkt(frame);
if (DEBUG) plog("DEBUG: missed ID %02X\n", frame.can_id);
break;
}
}
/**
* stmmac_xmit:
* @skb : the socket buffer
* @dev : device pointer
* Description : Tx entry point of the driver.
*/
static netdev_tx_t stmmac_xmit(struct sk_buff *skb, struct net_device *dev)
{
struct stmmac_priv *priv = netdev_priv(dev);
unsigned int txsize = priv->dma_tx_size;
unsigned int entry;
int i, csum_insertion = 0;
int nfrags = skb_shinfo(skb)->nr_frags;
struct dma_desc *desc, *first;
if (unlikely(stmmac_tx_avail(priv) < nfrags + 1)) {
if (!netif_queue_stopped(dev)) {
netif_stop_queue(dev);
/* This is a hard error, log it. */
pr_err("%s: BUG! Tx Ring full when queue awake\n",
__func__);
}
return NETDEV_TX_BUSY;
}
entry = priv->cur_tx % txsize;
#ifdef STMMAC_XMIT_DEBUG
if ((skb->len > ETH_FRAME_LEN) || nfrags)
pr_info("stmmac xmit:\n"
"\tskb addr %p - len: %d - nopaged_len: %d\n"
"\tn_frags: %d - ip_summed: %d - %s gso\n",
skb, skb->len, skb_headlen(skb), nfrags, skb->ip_summed,
!skb_is_gso(skb) ? "isn't" : "is");
#endif
csum_insertion = (skb->ip_summed == CHECKSUM_PARTIAL);
desc = priv->dma_tx + entry;
first = desc;
#ifdef STMMAC_XMIT_DEBUG
if ((nfrags > 0) || (skb->len > ETH_FRAME_LEN))
pr_debug("stmmac xmit: skb len: %d, nopaged_len: %d,\n"
"\t\tn_frags: %d, ip_summed: %d\n",
skb->len, skb_headlen(skb), nfrags, skb->ip_summed);
#endif
priv->tx_skbuff[entry] = skb;
if (unlikely(skb->len >= BUF_SIZE_4KiB)) {
entry = stmmac_handle_jumbo_frames(skb, dev, csum_insertion);
desc = priv->dma_tx + entry;
} else {
unsigned int nopaged_len = skb_headlen(skb);
desc->des2 = dma_map_single(priv->device, skb->data,
nopaged_len, DMA_TO_DEVICE);
priv->hw->desc->prepare_tx_desc(desc, 1, nopaged_len,
csum_insertion);
}
for (i = 0; i < nfrags; i++) {
skb_frag_t *frag = &skb_shinfo(skb)->frags[i];
int len = frag->size;
entry = (++priv->cur_tx) % txsize;
desc = priv->dma_tx + entry;
TX_DBG("\t[entry %d] segment len: %d\n", entry, len);
desc->des2 = dma_map_page(priv->device, frag->page,
frag->page_offset,
len, DMA_TO_DEVICE);
priv->tx_skbuff[entry] = NULL;
priv->hw->desc->prepare_tx_desc(desc, 0, len, csum_insertion);
wmb();
priv->hw->desc->set_tx_owner(desc);
}
/* Interrupt on completition only for the latest segment */
priv->hw->desc->close_tx_desc(desc);
#ifdef CONFIG_STMMAC_TIMER
/* Clean IC while using timer */
if (likely(priv->tm->enable))
priv->hw->desc->clear_tx_ic(desc);
#endif
wmb();
/* To avoid raise condition */
priv->hw->desc->set_tx_owner(first);
priv->cur_tx++;
#ifdef STMMAC_XMIT_DEBUG
if (netif_msg_pktdata(priv)) {
pr_info("stmmac xmit: current=%d, dirty=%d, entry=%d, "
"first=%p, nfrags=%d\n",
(priv->cur_tx % txsize), (priv->dirty_tx % txsize),
entry, first, nfrags);
display_ring(priv->dma_tx, txsize);
pr_info(">>> frame to be transmitted: ");
print_pkt(skb->data, skb->len);
}
#endif
if (unlikely(stmmac_tx_avail(priv) <= (MAX_SKB_FRAGS + 1))) {
TX_DBG("%s: stop transmitted packets\n", __func__);
netif_stop_queue(dev);
}
dev->stats.tx_bytes += skb->len;
skb_tx_timestamp(skb);
priv->hw->dma->enable_dma_transmission(priv->ioaddr);
return NETDEV_TX_OK;
}
static int stmmac_rx(struct stmmac_priv *priv, int limit)
{
unsigned int rxsize = priv->dma_rx_size;
unsigned int entry = priv->cur_rx % rxsize;
unsigned int next_entry;
unsigned int count = 0;
struct dma_desc *p = priv->dma_rx + entry;
struct dma_desc *p_next;
#ifdef STMMAC_RX_DEBUG
if (netif_msg_hw(priv)) {
pr_debug(">>> stmmac_rx: descriptor ring:\n");
display_ring(priv->dma_rx, rxsize);
}
#endif
count = 0;
while (!priv->hw->desc->get_rx_owner(p)) {
int status;
if (count >= limit)
break;
count++;
next_entry = (++priv->cur_rx) % rxsize;
p_next = priv->dma_rx + next_entry;
prefetch(p_next);
/* read the status of the incoming frame */
status = (priv->hw->desc->rx_status(&priv->dev->stats,
&priv->xstats, p));
if (unlikely(status == discard_frame))
priv->dev->stats.rx_errors++;
else {
struct sk_buff *skb;
int frame_len;
frame_len = priv->hw->desc->get_rx_frame_len(p);
/* ACS is set; GMAC core strips PAD/FCS for IEEE 802.3
* Type frames (LLC/LLC-SNAP) */
if (unlikely(status != llc_snap))
frame_len -= ETH_FCS_LEN;
#ifdef STMMAC_RX_DEBUG
if (frame_len > ETH_FRAME_LEN)
pr_debug("\tRX frame size %d, COE status: %d\n",
frame_len, status);
if (netif_msg_hw(priv))
pr_debug("\tdesc: %p [entry %d] buff=0x%x\n",
p, entry, p->des2);
#endif
skb = priv->rx_skbuff[entry];
if (unlikely(!skb)) {
pr_err("%s: Inconsistent Rx descriptor chain\n",
priv->dev->name);
priv->dev->stats.rx_dropped++;
break;
}
prefetch(skb->data - NET_IP_ALIGN);
priv->rx_skbuff[entry] = NULL;
skb_put(skb, frame_len);
dma_unmap_single(priv->device,
priv->rx_skbuff_dma[entry],
priv->dma_buf_sz, DMA_FROM_DEVICE);
#ifdef STMMAC_RX_DEBUG
if (netif_msg_pktdata(priv)) {
pr_info(" frame received (%dbytes)", frame_len);
print_pkt(skb->data, frame_len);
}
#endif
skb->protocol = eth_type_trans(skb, priv->dev);
if (unlikely(status == csum_none)) {
/* always for the old mac 10/100 */
skb_checksum_none_assert(skb);
netif_receive_skb(skb);
} else {
skb->ip_summed = CHECKSUM_UNNECESSARY;
napi_gro_receive(&priv->napi, skb);
}
priv->dev->stats.rx_packets++;
priv->dev->stats.rx_bytes += frame_len;
}
entry = next_entry;
p = p_next; /* use prefetched values */
}
stmmac_rx_refill(priv);
priv->xstats.rx_pkt_n += count;
return count;
}
void process_blocks(struct gprs_tbf *t, int ul)
{
uint8_t bsn, bsn2;
struct gprs_frag *f;
struct gprs_lime *l;
unsigned i, skip;
unsigned llc_len = 0;
uint8_t llc_data[65536];
uint8_t llc_first_bsn;
uint8_t llc_last_bsn = 0;
uint8_t li_off;
uint32_t current_fn;
/* get current "time", oldest unreassembled frag */
bsn = t->start_bsn;
while (t->frags[bsn].len == 0) {
bsn = (bsn+1)%128;
if (bsn == t->start_bsn) {
printf("no valid blocks in current TBF!\n");
fflush(stdout);
return;
}
}
current_fn = t->frags[bsn].fn;
t->start_bsn = bsn;
/* walk through framents, mark reassembled/used blocks */
skip = 0;
for (bsn = t->start_bsn; bsn != ((t->last_bsn+1)%128); bsn = (bsn+1)%128) {
/* get frament descriptor */
f = &t->frags[bsn];
printf(" bsn %d ", bsn);
fflush(stdout);
/* already processed or null */
if (!f->len) {
printf("null\n");
fflush(stdout);
llc_len = 0;
skip = 1;
continue;
}
/* check frament age */
if (too_old(current_fn, f->fn)) {
printf("old sement\n");
fflush(stdout);
llc_len = 0;
skip = 1;
continue;
}
/* update "time" */
current_fn = f->fn;
if (llc_len && !bsn_is_next(llc_last_bsn, bsn)) {
printf("missing bsn, previous %d\n", llc_last_bsn);
fflush(stdout);
llc_len = 0;
skip = 1;
continue;
}
/* check for multiple blocks/parts */
if (f->n_blocks == 0) {
/* check if first part of message */
if (!llc_len)
llc_first_bsn = bsn;
/* append data to buffer */
memcpy(&llc_data[llc_len], f->data, f->len);
llc_len += f->len;
llc_last_bsn = bsn;
/* last TBF block? (very rare condition) */
if (f->last) {
printf("end of TBF\n");
fflush(stdout);
print_pkt(llc_data, llc_len);
net_send_llc(llc_data, llc_len, ul);
/* reset all framents */
for (bsn2 = 0; bsn2 < 128; bsn2++) {
f = &t->frags[bsn2];
f->len = 0;
f->n_blocks = 0;
}
/* reset buffer state */
llc_len = 0;
t->start_bsn = 0;
}
} else {
/* multiple data parts */
li_off = 0;
for (i=0; i<f->n_blocks; i++) {
printf("\nlime %d\n", i);
fflush(stdout);
l = &f->blocks[i];
if (l->used) {
if (llc_len) {
// error!
printf("\nlime error!\n");
fflush(stdout);
llc_len = 0;
}
} else {
if (!llc_len)
llc_first_bsn = bsn;
/* append data to buffer */
memcpy(&llc_data[llc_len], &f->data[li_off], l->li);
llc_len += l->li;
llc_last_bsn = bsn;
if (!l->e || !l->m || (l->e && l->m)) {
/* message ends here */
printf("end of message reached\n");
fflush(stdout);
print_pkt(llc_data, llc_len);
net_send_llc(llc_data, llc_len, ul);
/* mark frags as used */
l->used = 1;
if (llc_first_bsn != bsn) {
}
llc_len = 0;
if (!skip)
t->start_bsn = bsn;
}
}
li_off += l->li;
}
/* is spare data valid? */
if (l->m) {
if (llc_len) {
printf("spare and buffer not empty!\n");
print_pkt(llc_data, llc_len);
fflush(stdout);
}
if ((f->len > li_off) && (f->len-li_off < 65536)) {
memcpy(llc_data, &f->data[li_off], f->len-li_off);
llc_len = f->len - li_off;
llc_first_bsn = bsn;
llc_last_bsn = bsn;
t->start_bsn = bsn;
}
}
}
}
/* shift window if needed */
if (((t->last_bsn - t->start_bsn) % 128) > 64) {
t->start_bsn = (t->last_bsn - 64) % 128;
printf("shifting window\n");
fflush(stdout);
}
}