Пример #1
0
void
handleReceivedListInterfacesCmd(struct S_Encapsulation_Data *pa_stReceiveData)
{
  EIP_UINT8 *pacCommBuf = pa_stReceiveData->m_acCurrentCommBufferPos;
  pa_stReceiveData->nData_length = 2;
  htols(0x0000, &pacCommBuf); /* copy Interface data to nmsg for sending */
}
Пример #2
0
/*   INT8 ListServices(struct S_Encapsulation_Data *pa_S_ReceiveData)
 *   generate reply with "Communications Services" + compatibility Flags.
 *      pa_S_ReceiveData pointer to structure with received data
 */
void
handleReceivedListServicesCmd(struct S_Encapsulation_Data *pa_stReceiveData)
{
  EIP_UINT8 *pacCommBuf = pa_stReceiveData->m_acCurrentCommBufferPos;

  pa_stReceiveData->nData_length = g_stInterfaceInformation.Length + 2;

  /* copy Interface data to nmsg for sending */
  htols(1, &pacCommBuf);
  htols(g_stInterfaceInformation.TypeCode, &pacCommBuf);
  htols((EIP_UINT16) (g_stInterfaceInformation.Length - 4), &pacCommBuf);
  htols(g_stInterfaceInformation.EncapsulationProtocolVersion, &pacCommBuf);
  htols(g_stInterfaceInformation.CapabilityFlags, &pacCommBuf);
  memcpy(pacCommBuf, g_stInterfaceInformation.NameofService,
      sizeof(g_stInterfaceInformation.NameofService));
}
Пример #3
0
int
encapsulate_data(struct S_Encapsulation_Data *pa_stSendData)
{
  EIP_UINT8 *acCommBuf = &(pa_stSendData->m_acCommBufferStart[2]);
  /*htols(pa_stSendData->nCommand_code, &pa_stSendData->pEncapsulation_Data);*/
  htols(pa_stSendData->nData_length, &acCommBuf);
  /*the CommBuf should already contain the correct session handle*/
  /*htoll(pa_stSendData->nSession_handle, &pa_stSendData->pEncapsulation_Data); */
  acCommBuf += 4;
  htoll(pa_stSendData->nStatus, &acCommBuf);
  /*the CommBuf should already contain the correct sender context*/
  /*memcpy(pa_stSendData->pEncapsulation_Data, pa_stSendData->anSender_context, SENDER_CONTEXT_SIZE);*/
  /*pa_stSendData->pEncapsulation_Data += SENDER_CONTEXT_SIZE + 2;*//* the plus 2 is for the options value*/
  /*the CommBuf should already contain the correct  options value*/
  /*htols((EIP_UINT16)pa_stSendData->nOptions, &pa_stSendData->pEncapsulation_Data);*/

  return ENCAPSULATION_HEADER_LENGTH + pa_stSendData->nData_length;
}
Пример #4
0
int
encapsulateListIdentyResponseMessage(EIP_BYTE *pa_pacCommBuf)
{
  EIP_UINT8 *pacCommBufRunner = pa_pacCommBuf;
  EIP_BYTE *acIdLenBuf;

  htols(1, &pacCommBufRunner); /* one item */
  htols(ITEM_ID_LISTIDENTITY, &pacCommBufRunner);

  acIdLenBuf = pacCommBufRunner;
  pacCommBufRunner += 2; /*at this place the real length will be inserted below*/

  htols(SUPPORTED_PROTOCOL_VERSION, &pacCommBufRunner);

  encapsulateIPAdress(OPENER_ETHERNET_PORT, Interface_Configuration.IPAddress,
      pacCommBufRunner);
  pacCommBufRunner += 8;

  memset(pacCommBufRunner, 0, 8);
  pacCommBufRunner += 8;

  htols(VendorID, &pacCommBufRunner);
  htols(DeviceType, &pacCommBufRunner);
  htols(ProductCode, &pacCommBufRunner);
  *(pacCommBufRunner)++ = Revison.MajorRevision;
  *(pacCommBufRunner)++ = Revison.MinorRevision;
  htols(ID_Status, &pacCommBufRunner);
  htoll(SerialNumber, &pacCommBufRunner);
  *pacCommBufRunner++ = (unsigned char) ProductName.Length;
  memcpy(pacCommBufRunner, ProductName.String, ProductName.Length);
  pacCommBufRunner += ProductName.Length;
  *pacCommBufRunner++ = 0xFF;

  htols(pacCommBufRunner - acIdLenBuf - 2, &acIdLenBuf); /* the -2 is for not counting the length field*/

  return pacCommBufRunner - pa_pacCommBuf;
}
Пример #5
0
void
handleReceivedListIdentityCmdUDP(int pa_nSocket,
    struct sockaddr_in *pa_pstFromAddr,
    struct S_Encapsulation_Data *pa_stReceiveData)
{  
  struct SDelayedEncapsulationMessage *pstDelayedMessageBuffer = NULL;
  unsigned int i;
  EIP_UINT8 *pacCommBuf;

  for (i = 0; i < ENCAP_NUMBER_OF_SUPPORTED_DELAYED_ENCAP_MESSAGES; i++)
    {
      if (-1 == g_stDelayedEncapsulationMessages[i].m_nSocket)
        {
          pstDelayedMessageBuffer = &(g_stDelayedEncapsulationMessages[i]);
          break;
        }
    }

  if (NULL != pstDelayedMessageBuffer)
    {
      pstDelayedMessageBuffer->m_nSocket = pa_nSocket;
      memcpy((&pstDelayedMessageBuffer->m_stSendToAddr), pa_pstFromAddr,
          sizeof(struct sockaddr_in));

      determineDelayTime(pa_stReceiveData->m_acCommBufferStart,
          pstDelayedMessageBuffer);

      memcpy(&(pstDelayedMessageBuffer->m_anMsg[0]),
          pa_stReceiveData->m_acCommBufferStart, ENCAPSULATION_HEADER_LENGTH);

      pstDelayedMessageBuffer->m_unMessageSize =
          encapsulateListIdentyResponseMessage(
              &(pstDelayedMessageBuffer->m_anMsg[ENCAPSULATION_HEADER_LENGTH]));

      pacCommBuf = pstDelayedMessageBuffer->m_anMsg + 2;
      htols(pstDelayedMessageBuffer->m_unMessageSize, &pacCommBuf);
      pstDelayedMessageBuffer->m_unMessageSize += ENCAPSULATION_HEADER_LENGTH;
    }
}
Пример #6
0
/*
 * see <linux-kernel-source>/Documentation/usb/usbmon.txt and 
 * <linux-kernel-source>/drivers/usb/mon/mon_text.c for urb string 
 * format description
 */
static int
usb_read_linux(pcap_t *handle, int max_packets, pcap_handler callback, u_char *user)
{
	/* see:
	* /usr/src/linux/Documentation/usb/usbmon.txt 
	* for message format
	*/
	struct pcap_usb_linux *handlep = handle->private;
	unsigned timestamp;
	int tag, cnt, ep_num, dev_addr, dummy, ret, urb_len, data_len;
	char etype, pipeid1, pipeid2, status[16], urb_tag, line[USB_LINE_LEN];
	char *string = line;
	u_char * rawdata = handle->buffer;
	struct pcap_pkthdr pkth;
	pcap_usb_header* uhdr = (pcap_usb_header*)handle->buffer;
	u_char urb_transfer=0;
	int incoming=0;

	/* ignore interrupt system call errors */
	do {
		ret = read(handle->fd, line, USB_LINE_LEN - 1);
		if (handle->break_loop)
		{
			handle->break_loop = 0;
			return -2;
		}
	} while ((ret == -1) && (errno == EINTR));
	if (ret < 0)
	{
		if (errno == EAGAIN)
			return 0;	/* no data there */

		snprintf(handle->errbuf, PCAP_ERRBUF_SIZE,
		    "Can't read from fd %d: %s", handle->fd, strerror(errno));
		return -1;
	}

	/* read urb header; %n argument may increment return value, but it's 
	* not mandatory, so does not count on it*/
	string[ret] = 0;
	ret = sscanf(string, "%x %d %c %c%c:%d:%d %s%n", &tag, &timestamp, &etype, 
		&pipeid1, &pipeid2, &dev_addr, &ep_num, status, 
		&cnt);
	if (ret < 8)
	{
		snprintf(handle->errbuf, PCAP_ERRBUF_SIZE,
		    "Can't parse USB bus message '%s', too few tokens (expected 8 got %d)",
		    string, ret);
		return -1;
	}
	uhdr->id = tag;
	uhdr->device_address = dev_addr;
	uhdr->bus_id = handlep->bus_index;
	uhdr->status = 0;
	string += cnt;

	/* don't use usbmon provided timestamp, since it have low precision*/
	if (gettimeofday(&pkth.ts, NULL) < 0) 
	{
		snprintf(handle->errbuf, PCAP_ERRBUF_SIZE,
			"Can't get timestamp for message '%s' %d:%s", 
			string, errno, strerror(errno));
		return -1;
	}
	uhdr->ts_sec = pkth.ts.tv_sec;
	uhdr->ts_usec = pkth.ts.tv_usec;

	/* parse endpoint information */
	if (pipeid1 == 'C')
		urb_transfer = URB_CONTROL;
	else if (pipeid1 == 'Z')
		urb_transfer = URB_ISOCHRONOUS;
	else if (pipeid1 == 'I')
		urb_transfer = URB_INTERRUPT;
	else if (pipeid1 == 'B')
		urb_transfer = URB_BULK;
	if (pipeid2 == 'i') {
		ep_num |= URB_TRANSFER_IN;
		incoming = 1;
	}
	if (etype == 'C')
		incoming = !incoming;

	/* direction check*/
	if (incoming)
	{
		if (handle->direction == PCAP_D_OUT)
			return 0;
	}
	else
		if (handle->direction == PCAP_D_IN)
			return 0;
	uhdr->event_type = etype;
	uhdr->transfer_type = urb_transfer;
	uhdr->endpoint_number = ep_num;
	pkth.caplen = sizeof(pcap_usb_header);
	rawdata += sizeof(pcap_usb_header);

	/* check if this is a setup packet */
	ret = sscanf(status, "%d", &dummy);
	if (ret != 1)
	{
		/* this a setup packet, setup data can be filled with underscore if
		* usbmon has not been able to read them, so we must parse this fields as 
		* strings */
		pcap_usb_setup* shdr;
		char str1[3], str2[3], str3[5], str4[5], str5[5];
		ret = sscanf(string, "%s %s %s %s %s%n", str1, str2, str3, str4, 
		str5, &cnt);
		if (ret < 5)
		{
			snprintf(handle->errbuf, PCAP_ERRBUF_SIZE,
				"Can't parse USB bus message '%s', too few tokens (expected 5 got %d)",
				string, ret);
			return -1;
		}
		string += cnt;

		/* try to convert to corresponding integer */
		shdr = &uhdr->setup;
		shdr->bmRequestType = strtoul(str1, 0, 16);
		shdr->bRequest = strtoul(str2, 0, 16);
		shdr->wValue = htols(strtoul(str3, 0, 16));
		shdr->wIndex = htols(strtoul(str4, 0, 16));
		shdr->wLength = htols(strtoul(str5, 0, 16));

		uhdr->setup_flag = 0;
	}
	else 
		uhdr->setup_flag = 1;

	/* read urb data */
	ret = sscanf(string, " %d%n", &urb_len, &cnt);
	if (ret < 1)
	{
		snprintf(handle->errbuf, PCAP_ERRBUF_SIZE,
		  "Can't parse urb length from '%s'", string);
		return -1;
	}
	string += cnt;

	/* urb tag is not present if urb length is 0, so we can stop here 
	 * text parsing */
	pkth.len = urb_len+pkth.caplen;
	uhdr->urb_len = urb_len;
	uhdr->data_flag = 1;
	data_len = 0;
	if (uhdr->urb_len == 0)
		goto got;

	/* check for data presence; data is present if and only if urb tag is '=' */
	if (sscanf(string, " %c", &urb_tag) != 1)
	{
		snprintf(handle->errbuf, PCAP_ERRBUF_SIZE,
			"Can't parse urb tag from '%s'", string);
		return -1;
	}

	if (urb_tag != '=') 
		goto got;

	/* skip urb tag and following space */
	string += 3;

	/* if we reach this point we got some urb data*/
	uhdr->data_flag = 0;

	/* read all urb data; if urb length is greater then the usbmon internal 
	 * buffer length used by the kernel to spool the URB, we get only
	 * a partial information.
	 * At least until linux 2.6.17 there is no way to set usbmon intenal buffer
	 * length and default value is 130. */
	while ((string[0] != 0) && (string[1] != 0) && (pkth.caplen < handle->snapshot))
	{
		rawdata[0] = ascii_to_int(string[0]) * 16 + ascii_to_int(string[1]);
		rawdata++;
		string+=2;
		if (string[0] == ' ')
			string++;
		pkth.caplen++;
		data_len++;
	}

got:
	uhdr->data_len = data_len;
	if (pkth.caplen > handle->snapshot)
		pkth.caplen = handle->snapshot;

	if (handle->fcode.bf_insns == NULL ||
	    bpf_filter(handle->fcode.bf_insns, handle->buffer,
	      pkth.len, pkth.caplen)) {
		handlep->packets_read++;
		callback(user, &pkth, handle->buffer);
		return 1;
	}
	return 0;	/* didn't pass filter */
}
static int write_header(FILE *f)
{
	/* Samples per second (always 8000 for this format). */
	unsigned int sample_rate = htoll(8000);
	/* Bytes per second (always 1625 for this format). */
	unsigned int byte_sample_rate = htoll(1625);
	/* This is the size of the "fmt " subchunk */
	unsigned int fmtsize = htoll(20);
	/* WAV #49 */
	unsigned short fmt = htols(49);
	/* Mono = 1 channel */
	unsigned short chans = htols(1);
	/* Each block of data is exactly 65 bytes in size. */
	unsigned int block_align = htoll(MSGSM_FRAME_SIZE);
	/* Not actually 2, but rounded up to the nearest bit */
	unsigned short bits_per_sample = htols(2);
	/* Needed for compressed formats */
	unsigned short extra_format = htols(MSGSM_SAMPLES);
	/* This is the size of the "fact" subchunk */
	unsigned int factsize = htoll(4);
	/* Number of samples in the data chunk */
	unsigned int num_samples = htoll(0);
	/* Number of bytes in the data chunk */
	unsigned int size = htoll(0);
	/* Write a GSM header, ignoring sizes which will be filled in later */

	/*  0: Chunk ID */
	if (fwrite("RIFF", 1, 4, f) != 4) {
		ast_log(LOG_WARNING, "Unable to write header\n");
		return -1;
	}
	/*  4: Chunk Size */
	if (fwrite(&size, 1, 4, f) != 4) {
		ast_log(LOG_WARNING, "Unable to write header\n");
		return -1;
	}
	/*  8: Chunk Format */
	if (fwrite("WAVE", 1, 4, f) != 4) {
		ast_log(LOG_WARNING, "Unable to write header\n");
		return -1;
	}
	/* 12: Subchunk 1: ID */
	if (fwrite("fmt ", 1, 4, f) != 4) {
		ast_log(LOG_WARNING, "Unable to write header\n");
		return -1;
	}
	/* 16: Subchunk 1: Size (minus 8) */
	if (fwrite(&fmtsize, 1, 4, f) != 4) {
		ast_log(LOG_WARNING, "Unable to write header\n");
		return -1;
	}
	/* 20: Subchunk 1: Audio format (49) */
	if (fwrite(&fmt, 1, 2, f) != 2) {
		ast_log(LOG_WARNING, "Unable to write header\n");
		return -1;
	}
	/* 22: Subchunk 1: Number of channels */
	if (fwrite(&chans, 1, 2, f) != 2) {
		ast_log(LOG_WARNING, "Unable to write header\n");
		return -1;
	}
	/* 24: Subchunk 1: Sample rate */
	if (fwrite(&sample_rate, 1, 4, f) != 4) {
		ast_log(LOG_WARNING, "Unable to write header\n");
		return -1;
	}
	/* 28: Subchunk 1: Byte rate */
	if (fwrite(&byte_sample_rate, 1, 4, f) != 4) {
		ast_log(LOG_WARNING, "Unable to write header\n");
		return -1;
	}
	/* 32: Subchunk 1: Block align */
	if (fwrite(&block_align, 1, 4, f) != 4) {
		ast_log(LOG_WARNING, "Unable to write header\n");
		return -1;
	}
	/* 36: Subchunk 1: Bits per sample */
	if (fwrite(&bits_per_sample, 1, 2, f) != 2) {
		ast_log(LOG_WARNING, "Unable to write header\n");
		return -1;
	}
	/* 38: Subchunk 1: Extra format bytes */
	if (fwrite(&extra_format, 1, 2, f) != 2) {
		ast_log(LOG_WARNING, "Unable to write header\n");
		return -1;
	}
	/* 40: Subchunk 2: ID */
	if (fwrite("fact", 1, 4, f) != 4) {
		ast_log(LOG_WARNING, "Unable to write header\n");
		return -1;
	}
	/* 44: Subchunk 2: Size (minus 8) */
	if (fwrite(&factsize, 1, 4, f) != 4) {
		ast_log(LOG_WARNING, "Unable to write header\n");
		return -1;
	}
	/* 48: Subchunk 2: Number of samples */
	if (fwrite(&num_samples, 1, 4, f) != 4) {
		ast_log(LOG_WARNING, "Unable to write header\n");
		return -1;
	}
	/* 52: Subchunk 3: ID */
	if (fwrite("data", 1, 4, f) != 4) {
		ast_log(LOG_WARNING, "Unable to write header\n");
		return -1;
	}
	/* 56: Subchunk 3: Size */
	if (fwrite(&size, 1, 4, f) != 4) {
		ast_log(LOG_WARNING, "Unable to write header\n");
		return -1;
	}
	return 0;
}
Пример #8
0
static int write_header(int fd)
{
	unsigned int hz=htoll(8000);
	unsigned int bhz = htoll(1625);
	unsigned int hs = htoll(20);
	unsigned short fmt = htols(49);
	unsigned short chans = htols(1);
	unsigned int fhs = htoll(4);
	unsigned int x_1 = htoll(65);
	unsigned short x_2 = htols(2);
	unsigned short x_3 = htols(320);
	unsigned int y_1 = htoll(20160);
	unsigned int size = htoll(0);
	/* Write a GSM header, ignoring sizes which will be filled in later */
	if (write(fd, "RIFF", 4) != 4) {
		ast_log(LOG_WARNING, "Unable to write header\n");
		return -1;
	}
	if (write(fd, &size, 4) != 4) {
		ast_log(LOG_WARNING, "Unable to write header\n");
		return -1;
	}
	if (write(fd, "WAVEfmt ", 8) != 8) {
		ast_log(LOG_WARNING, "Unable to write header\n");
		return -1;
	}
	if (write(fd, &hs, 4) != 4) {
		ast_log(LOG_WARNING, "Unable to write header\n");
		return -1;
	}
	if (write(fd, &fmt, 2) != 2) {
		ast_log(LOG_WARNING, "Unable to write header\n");
		return -1;
	}
	if (write(fd, &chans, 2) != 2) {
		ast_log(LOG_WARNING, "Unable to write header\n");
		return -1;
	}
	if (write(fd, &hz, 4) != 4) {
		ast_log(LOG_WARNING, "Unable to write header\n");
		return -1;
	}
	if (write(fd, &bhz, 4) != 4) {
		ast_log(LOG_WARNING, "Unable to write header\n");
		return -1;
	}
	if (write(fd, &x_1, 4) != 4) {
		ast_log(LOG_WARNING, "Unable to write header\n");
		return -1;
	}
	if (write(fd, &x_2, 2) != 2) {
		ast_log(LOG_WARNING, "Unable to write header\n");
		return -1;
	}
	if (write(fd, &x_3, 2) != 2) {
		ast_log(LOG_WARNING, "Unable to write header\n");
		return -1;
	}
	if (write(fd, "fact", 4) != 4) {
		ast_log(LOG_WARNING, "Unable to write header\n");
		return -1;
	}
	if (write(fd, &fhs, 4) != 4) {
		ast_log(LOG_WARNING, "Unable to write header\n");
		return -1;
	}
	if (write(fd, &y_1, 4) != 4) {
		ast_log(LOG_WARNING, "Unable to write header\n");
		return -1;
	}
	if (write(fd, "data", 4) != 4) {
		ast_log(LOG_WARNING, "Unable to write header\n");
		return -1;
	}
	if (write(fd, &size, 4) != 4) {
		ast_log(LOG_WARNING, "Unable to write header\n");
		return -1;
	}
	return 0;
}
Пример #9
0
// sample rate 8000, 12000, 16000, 24000
int wav_write_header(FILE *f, int samplerate, int stereo)
{
	unsigned int hz=htoll(samplerate);
	unsigned int bhz = htoll(samplerate*2*(stereo ? 2 : 1)); // 2 bytes per sample and 2 channels
	unsigned int hs = htoll(16);	// 16bit
	unsigned short fmt = htols(1);
	unsigned short chans = htols((stereo ? 2 : 1));
	unsigned short bysam = htols(2*(stereo ? 2 : 1));
	unsigned short bisam = htols(16);
	unsigned int size = htoll(0);
	/* Write a wav header, ignoring sizes which will be filled in later */
	fseek(f,0,SEEK_SET);
	if (fwrite("RIFF", 1, 4, f) != 4) {
		//log(LOG_WARNING, "Unable to write header\n");
		return -1;
	}
	if (fwrite(&size, 1, 4, f) != 4) {
		//log(LOG_WARNING, "Unable to write header\n");
		return -1;
	}
	if (fwrite("WAVEfmt ", 1, 8, f) != 8) {
		//log(LOG_WARNING, "Unable to write header\n");
		return -1;
	}
	if (fwrite(&hs, 1, 4, f) != 4) {
		//log(LOG_WARNING, "Unable to write header\n");
		return -1;
	}
	if (fwrite(&fmt, 1, 2, f) != 2) {
		//log(LOG_WARNING, "Unable to write header\n");
		return -1;
	}
	if (fwrite(&chans, 1, 2, f) != 2) {
		//log(LOG_WARNING, "Unable to write header\n");
		return -1;
	}
	if (fwrite(&hz, 1, 4, f) != 4) {
		///log(LOG_WARNING, "Unable to write header\n");
		return -1;
	}
	if (fwrite(&bhz, 1, 4, f) != 4) {
		//log(LOG_WARNING, "Unable to write header\n");
		return -1;
	}
	if (fwrite(&bysam, 1, 2, f) != 2) {
		//log(LOG_WARNING, "Unable to write header\n");
		return -1;
	}
	if (fwrite(&bisam, 1, 2, f) != 2) {
		//log(LOG_WARNING, "Unable to write header\n");
		return -1;
	}
	if (fwrite("data", 1, 4, f) != 4) {
		//log(LOG_WARNING, "Unable to write header\n");
		return -1;
	}
	if (fwrite(&size, 1, 4, f) != 4) {
		//log(LOG_WARNING, "Unable to write header\n");
		return -1;
	}
	return 0;
}