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 */
}
/* 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));
}
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;
}
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;
}
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;
}
}
/*
* 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, ×tamp, &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;
}
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;
}
// 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;
}