gsm0710muxd.c

Linux GPRS 模块多路复用实现多个虚拟串口

			{
				end = buf->endp - data;
				if (frame->length > end)
				{
					memcpy(frame->data, data, end);
					memcpy(frame->data + end, buf->data, frame->length - end);
					data = buf->data + (frame->length - end);
				}
				else
				{
					memcpy(frame->data, data, frame->length);
					data += frame->length;
					if (data == buf->endp)
						data = buf->data;
				}
				if (GSM0710_FRAME_IS(GSM0710_TYPE_UI, frame))
				{
					for (end = 0; end < frame->length; end++)
						fcs = r_crctable[fcs ^ (frame->data[end])];
				}
			}
			else
			{
				LOG(LOG_ALERT, "Out of memory, when allocating space for frame data");
				frame->length = 0;
			}
		}
//check FCS
		if (r_crctable[fcs ^ (*data)] != 0xCF)
		{
			LOG(LOG_WARNING, "Dropping frame: FCS doesn't match");
			destroy_frame(frame);
			buf->flag_found = 0;
			buf->dropped_count++;
			buf->readp = data;
			return gsm0710_base_buffer_get_frame(buf);
		}
		else
		{
//check end flag
			gsm0710_buffer_inc(buf, data);
			if (*data != GSM0710_FRAME_FLAG)
			{
				LOG(LOG_WARNING, "Dropping frame: End flag not found. Instead: %d", *data);
				destroy_frame(frame);
				buf->flag_found = 0;
				buf->dropped_count++;
				buf->readp = data;
				return gsm0710_base_buffer_get_frame(buf);
			}
			else
				buf->received_count++;
			gsm0710_buffer_inc(buf, data);
		}
		buf->readp = data;
	}
	return frame;
}

/* Gets a advanced option frame from buffer. You have to remember to free this frame
 * when it's not needed anymore
 *
 * PARAMS:
 * buf - the buffer, where the frame is extracted
 * RETURNS:
 * frame or null, if there isn't ready frame with given index
 */
static GSM0710_Frame *gsm0710_advanced_buffer_get_frame(
	GSM0710_Buffer * buf)
{
	LOG(LOG_DEBUG, "Enter");
l_begin:
//Find start flag
	while (!buf->flag_found && gsm0710_buffer_length(buf) > 0)
	{
		if (*buf->readp == GSM0710_FRAME_ADV_FLAG)
		{
			buf->flag_found = 1;
			buf->adv_length = 0;
			buf->adv_found_esc = 0;
		}
		gsm0710_buffer_inc(buf, buf->readp);
	}
	if (!buf->flag_found)// no frame started
		return NULL;
	if (0 == buf->adv_length)
//skip empty frames (this causes troubles if we're using DLC 62)
		while (gsm0710_buffer_length(buf) > 0 && (*buf->readp == GSM0710_FRAME_ADV_FLAG))
			gsm0710_buffer_inc(buf, buf->readp);
	while (gsm0710_buffer_length(buf) > 0)
	{
		if (!buf->adv_found_esc && GSM0710_FRAME_ADV_FLAG == *(buf->readp))
		{// closing flag found
			GSM0710_Frame *frame = NULL;
			unsigned char *data = buf->adv_data;
			unsigned char fcs = 0xFF;
			gsm0710_buffer_inc(buf, buf->readp);
			if (buf->adv_length < 3)
			{
				LOG(LOG_WARNING, "Too short adv frame, length:%d", buf->adv_length);
				buf->flag_found = 0;
				goto l_begin;
			}
			if ((frame = (GSM0710_Frame*)malloc(sizeof(GSM0710_Frame))) != NULL)
			{
				frame->channel = ((data[0] & 252) >> 2);
				fcs = r_crctable[fcs ^ data[0]];
				frame->control = data[1];
				fcs = r_crctable[fcs ^ data[1]];
				frame->length = buf->adv_length - 3;
			}
			else
				LOG(LOG_ALERT, "Out of memory, when allocating space for frame");
//extract data
			if (frame->length > 0)
			{
				if ((frame->data = (unsigned char *) malloc(sizeof(char) * frame->length)))
				{
					memcpy(frame->data, data + 2, frame->length);
					if (GSM0710_FRAME_IS(GSM0710_TYPE_UI, frame))
					{
						int i;
						for (i = 0; i < frame->length; ++i)
							fcs = r_crctable[fcs ^ (frame->data[i])];
					}
				}
				else
				{
					LOG(LOG_ALERT, "Out of memory, when allocating space for frame data");
					buf->flag_found = 0;
					goto l_begin;
				}
			}
//check FCS
			if (r_crctable[fcs ^ data[buf->adv_length - 1]] != 0xCF)
			{
				LOG(LOG_WARNING, "Dropping frame: FCS doesn't match");
				destroy_frame(frame);
				buf->flag_found = 0;
				buf->dropped_count++;
				goto l_begin;
			}
			else
			{
				buf->received_count++;
				buf->flag_found = 0;
				LOG(LOG_DEBUG, "Leave success");
				return frame;
			}
		}
		if (buf->adv_length >= sizeof(buf->adv_data))
		{
			LOG(LOG_WARNING, "Too long adv frame, length:%d", buf->adv_length);
			buf->flag_found = 0;
			buf->dropped_count++;
			goto l_begin;
		}
		if (buf->adv_found_esc)
		{
			buf->adv_data[buf->adv_length] = *(buf->readp) ^ GSM0710_FRAME_ADV_ESC_COPML;
			buf->adv_length++;
			buf->adv_found_esc = 0;
		}
		else if (GSM0710_FRAME_ADV_ESC == *(buf->readp))
			buf->adv_found_esc = 1;
		else
		{
			buf->adv_data[buf->adv_length] = *(buf->readp);
			buf->adv_length++;
		}
		gsm0710_buffer_inc(buf, buf->readp);
	}
	return NULL;
}

/**
 * Returns 1 if found, 0 otherwise. needle must be null-terminated.
 * strstr might not work because WebBox sends garbage before the first
 * OK
 */
static int memstr(
	const char *haystack,
	int length,
	const char *needle)
{
	int i;
	int j = 0;
	if (needle[0] == '\0')
		return 1;
	for (i = 0; i < length; i++)
		if (needle[j] == haystack[i])
		{
			j++;
			if (needle[j] == '\0') // Entire needle was found
				return 1;
		}
		else
			j = 0;
	return 0;
}

/*
 * Sends an AT-command to a given serial port and waits for reply. 
 * PARAMS: fd - file descriptor cmd - command to - how many
 * seconds to wait for response RETURNS: 
 * 0 on success (OK-response), -1 otherwise
 */
static int chat(
	int serial_device_fd,
	char *cmd,
	int to)
{
	LOG(LOG_DEBUG, "Enter");
	unsigned char buf[1024];
	int sel;
	int len;
	int wrote = 0;
	syslogdump(">s ", (unsigned char *) cmd, strlen(cmd));
	SYSCHECK(wrote = write(serial_device_fd, cmd, strlen(cmd)));
	LOG(LOG_DEBUG, "Wrote %d bytes", wrote);
	SYSCHECK(tcdrain(serial_device_fd));

	fd_set rfds;
	FD_ZERO(&rfds);
	FD_SET(serial_device_fd, &rfds);
	struct timeval timeout;
	timeout.tv_sec = to;
	timeout.tv_usec = 0;
	do
	{
		SYSCHECK(sel = select(serial_device_fd + 1, &rfds, NULL, NULL, &timeout));
		LOG(LOG_DEBUG, "Selected %d", sel);
		if (FD_ISSET(serial_device_fd, &rfds))
		{
			memset(buf, 0, sizeof(buf));
			len = read(serial_device_fd, buf, sizeof(buf));
			SYSCHECK(len);
			LOG(LOG_DEBUG, "Read %d bytes from serial device", len);
			syslogdump("<s ", buf, len);
			errno = 0;
			if (memstr((char *) buf, len, "OK"))
			{
				LOG(LOG_DEBUG, "Received OK");
				return 0;
			}
			if (memstr((char *) buf, len, "ERROR"))
			{
				LOG(LOG_DEBUG, "Received ERROR");
				return -1;
			}
		}
	} while (sel);
	return -1;
}

/*
 * Handles commands received from the control channel.
 */
static int handle_command(
	GSM0710_Frame * frame)
{
	LOG(LOG_DEBUG, "Enter");
	unsigned char type, signals;
	int length = 0, i, type_length, channel, supported = 1;
	unsigned char *response;
//struct ussp_operation op;
	if (frame->length > 0)
	{
		type = frame->data[0];// only a byte long types are handled now skip extra bytes
		for (i = 0; (frame->length > i && (frame->data[i] & GSM0710_EA) == 0); i++);
		i++;
		type_length = i;
		if ((type & GSM0710_CR) == GSM0710_CR)
		{
//command not ack extract frame length
			while (frame->length > i)
			{
				length = (length * 128) + ((frame->data[i] & 254) >> 1);
				if ((frame->data[i] & 1) == 1)
					break;
				i++;
			}
			i++;
			switch ((type & ~GSM0710_CR))
			{
			case GSM0710_CONTROL_CLD:
				LOG(LOG_INFO, "The mobile station requested mux-mode termination");
				serial.state = MUX_STATE_CLOSING;				
				break;
			case GSM0710_CONTROL_PSC:
				LOG(LOG_DEBUG, "Power Service Control command: ***");
				LOG(LOG_DEBUG, "Frame->data = %s / frame->length = %d", frame->data + i, frame->length - i);
			break;
			case GSM0710_CONTROL_TEST:
				LOG(LOG_DEBUG, "Test command: ");
				LOG(LOG_DEBUG, "Frame->data = %s / frame->length = %d", frame->data + i, frame->length - i);
				//serial->ping_number = 0;
				break;
			case GSM0710_CONTROL_MSC:
				if (i + 1 < frame->length)
				{
					channel = ((frame->data[i] & 252) >> 2);
					i++;
					signals = (frame->data[i]);
//op.op = USSP_MSC;
//op.arg = USSP_RTS;
//op.len = 0;
					LOG(LOG_DEBUG, "Modem status command on channel %d", channel);
					if ((signals & GSM0710_SIGNAL_FC) == GSM0710_SIGNAL_FC)
						LOG(LOG_DEBUG, "No frames allowed");
					else
					{
//op.arg |= USSP_CTS;
						LOG(LOG_DEBUG, "Frames allowed");
					}
					if ((signals & GSM0710_SIGNAL_RTC) == GSM0710_SIGNAL_RTC)
					{
//op.arg |= USSP_DSR;
						LOG(LOG_DEBUG, "Signal RTC");
					}
					if ((signals & GSM0710_SIGNAL_IC) == GSM0710_SIGNAL_IC)
					{
//op.arg |= USSP_RI;
						LOG(LOG_DEBUG, "Signal Ring");
					}
					if ((signals & GSM0710_SIGNAL_DV) == GSM0710_SIGNAL_DV)
					{
//op.arg |= USSP_DCD;
						LOG(LOG_DEBUG, "Signal DV");
					}
//if (channel > 0)
//write(channellist[channel].fd, &op,
//sizeof(op));
				}
				else
					LOG(LOG_ERR, "Modem status command, but no info. i: %d, len: %d, data-len: %d",
						i, length, frame->length);
				break;
			default:
				LOG(LOG_ALERT, "Unknown command (%d) from the control channel", type);
				if ((response = malloc(sizeof(char) * (2 + type_length))) != NULL)
				{
					i = 0;
					response[i++] = GSM0710_CONTROL_NSC;
					type_length &= 127; //supposes that type length is less than 128
					response[i++] = GSM0710_EA | (type_length << 1);
					while (type_length--)
					{
						response[i] = frame->data[i - 2];
						i++;
					}
					write_frame(0, response, i, GSM0710_TYPE_UIH);
					free(response);
					supported = 0;
				}
				else
					LOG(LOG_ALERT, "Out of memory, when allocating space for response");
				break;
			}
			if (supported)
			{
//acknowledge the command
				frame->data[0] = frame->data[0] & ~GSM0710_CR;
				write_frame(0, frame->data, frame->length, GSM0710_TYPE_UIH);
			}
		}
		else
		{
//received ack for a command
			if (GSM0710_COMMAND_IS(type, GSM0710_CONTROL_NSC))
				LOG(LOG_ERR, "The mobile station didn't support the command sent");
			else
				LOG(LOG_DEBUG, "Command acknowledged by the mobile station");
		}
	}
	return 0;
}

/*
 * Extracts and handles frames from the receiver buffer. PARAMS: buf
 * - the receiver buffer
 */
int extract_frames(
	GSM0710_Buffer* buf)
{
	LOG(LOG_DEBUG, "Enter");
//version test for Siemens terminals to enable version 2 functions
	int frames_extracted = 0;
	GSM0710_Frame *frame;
	while ((frame = cmux_mode
		? gsm0710_advanced_buffer_get_frame(buf)
		: gsm0710_base_buffer_get_frame(buf)))
	{
		frames_extracted++;
		if ((GSM0710_FRAME_IS(GSM0710_TYPE_UI, frame) || GSM0710_FRAME_IS(GSM0710_TYPE_UIH, frame)))
		{
			LOG(LOG_DEBUG, "Frame is UI or UIH");
			if (frame->channel > 0)
			{
				LOG(LOG_DEBUG, "Frame channel > 0, pseudo channel");
//data from logical channel
				write(channellist[frame->channel].fd, frame->data, frame->length);
			}
			else
			{
//control channel command
				LOG(LOG_DEBUG, "Frame channel == 0, control channel command");
				handle_command(frame);
			}
		}
		else
		{
//not an information frame
			LOG(LOG_DEBUG, "Not an information frame");
			switch ((frame->control & ~GSM0710_PF))
			{
			case GSM0710_TYPE_UA:
				LOG(LOG_DEBUG, "Frame is UA");
				if (channellist[frame->channel].opened)
				{
					SYSCHECK(logical_channel_close(channellist+frame->channel));
					LOG(LOG_INFO, "Logical channel %d for %s closed",
						frame->channel, channellist[frame->channel].origin);
				}
				else
				{
					channellist[frame->channel].opened = 1;
					if (frame->channel == 0)
					{
						LOG(LOG_DEBUG, "Control channel opened");
						//send version Siemens version test
						//static unsigned char version_test[] = "\x23\x21\x04TEMUXVERSION2\0";
						//write_frame(0, version_test, sizeof(version_test), GSM0710_TYPE_UIH);
					}
					else
						LOG(LOG_INFO, "Logical channel %d opened", frame->channel);
				}
				break;
			case GSM0710_TYPE_DM:
				if (channellist[frame->channel].opened)
				{
					SYSCHECK(logical_channel_close(channellist+frame->channel));
					LOG(LOG_INFO, "DM received, so the channel %d for %s was already closed",
						frame->channel, channellist[frame->channel].origin);
				}
				else
				{
					if (frame->channel == 0)
					{
						LOG(LOG_INFO, "Couldn't open control channel.\n->Terminating");
						serial.state = MUX_STATE_CLOSING;				
//close channels
					}
					else
						LOG(LOG_INFO, "Logical channel %d for %s couldn't be opened", frame->channel, channellist[frame->channel].origin);
				}
				break;
			case GSM0710_TYPE_DISC:
				if (channellist[frame->channel].opened)
				{
					channellist[frame->channel].opened = 0;
					write_frame(frame->channel, NULL, 0, GSM0710_TYPE_UA | GSM0710_PF);
					if (frame->channel == 0)
					{
						serial.state = MUX_STATE_CLOSING;				
						LOG(LOG_INFO, "Control channel closed");
					}
					else
						LOG(LOG_INFO, "Logical channel %d for %s closed", frame->channel, channellist[frame->channel].origin);
				}
				else
				{
//channel already closed
					LOG(LOG_WARNING, "Received DISC even though channel %d for %s was already closed",
							frame->channel, channellist[frame->channel].origin);
					write_frame(frame->channel, NULL, 0, GSM0710_TYPE_DM | GSM0710_PF);
				}
				break;
			case GSM0710_TYPE_SABM:
//channel open request
				if (channellist[frame->channel].opened)
				{
					if (frame->channel == 0)
						LOG(LOG_INFO, "Control channel opened");
					else
						LOG(LOG_INFO, "Logical channel %d for %s opened",
							frame->channel, channellist[frame->channel].origin);