Exemplo n.º 1
0
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;
}
Exemplo n.º 2
0
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");
  }
}
Exemplo n.º 3
0
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);
}
Exemplo n.º 4
0
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);
}
Exemplo n.º 5
0
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 *) &eth_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 *) &eth_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;
}
Exemplo n.º 8
0
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;
}
Exemplo n.º 9
0
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);
            }
        }
    }
}
Exemplo n.º 10
0
// 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;
  }
}
Exemplo n.º 11
0
/**
 *  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;
}
Exemplo n.º 12
0
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;
}
Exemplo n.º 13
0
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);
	}
}