1/*
2 * Copyright 2003-2006, Waldemar Kornewald <wkornew@gmx.net>
3 * Distributed under the terms of the MIT License.
4 */
5
6#include <cstdio>
7
8#include "ModemDevice.h"
9#include "ACFCHandler.h"
10#include "fcs.h"
11
12#include <core_funcs.h>
13#include <unistd.h>
14#include <termios.h>
15	// for port settings
16
17// from libkernelppp
18#include <settings_tools.h>
19
20
21#if DEBUG
22static char sDigits[] = "0123456789ABCDEF";
23void
24dump_packet(struct mbuf *packet)
25{
26	if(!packet)
27		return;
28
29	uint8 *data = mtod(packet, uint8*);
30	uint8 buffer[33];
31	uint8 bufferIndex = 0;
32
33	TRACE("Dumping packet;len=%ld;pkthdr.len=%d\n", packet->m_len,
34		packet->m_flags & M_PKTHDR ? packet->m_pkthdr.len : -1);
35
36	for(uint32 index = 0; index < packet->m_len; index++) {
37		buffer[bufferIndex++] = sDigits[data[index] >> 4];
38		buffer[bufferIndex++] = sDigits[data[index] & 0x0F];
39		if(bufferIndex == 32 || index == packet->m_len - 1) {
40			buffer[bufferIndex] = 0;
41			TRACE("%s\n", buffer);
42			bufferIndex = 0;
43		}
44	}
45}
46#endif
47
48
49status_t
50modem_put_line(int32 handle, const char *string, int32 length)
51{
52	char line[128];
53	if(length > 126)
54		return -1;
55
56	sprintf(line, "%s\r", string);
57	return write(handle, line, length + 1);
58}
59
60
61status_t
62modem_get_line(int32 handle, char *string, int32 length, const char *echo)
63{
64	if(!string || length < 40)
65		return -1;
66
67	int32 result, position = 0;
68
69	while(position < length) {
70		result = read(handle, string + position, 1);
71		if(result < 0)
72			return -1;
73		else if(result == 1) {
74			if(string[position] == '\r') {
75				string[position] = 0;
76				if(!strcasecmp(string, echo)) {
77					position = 0;
78					continue;
79				}
80
81				return position;
82			}
83
84			position++;
85		}
86	}
87
88	return -1;
89}
90
91
92static
93status_t
94worker_thread(void *data)
95{
96	ModemDevice *device = (ModemDevice*) data;
97	int32 handle = device->Handle();
98	uint8 buffer[MODEM_MTU];
99
100	// send init string
101	if(modem_put_line(handle, device->InitString(), strlen(device->InitString())) < 0
102			|| modem_get_line(handle, (char*) buffer, sizeof(buffer),
103					device->InitString()) < 0
104			|| strcmp((char*) buffer, "OK")) {
105		device->FailedDialing();
106		return B_ERROR;
107	}
108
109	// send dial string
110	if(modem_put_line(handle, device->DialString(), strlen(device->DialString())) < 0
111			|| modem_get_line(handle, (char*) buffer, sizeof(buffer),
112					device->DialString()) < 0
113			|| strncmp((char*) buffer, "CONNECT", 7)) {
114		device->FailedDialing();
115		return B_ERROR;
116	}
117
118	if(strlen((char*) buffer) > 8)
119		device->SetSpeed(atoi((char*) buffer + 8));
120	else
121		device->SetSpeed(19200);
122
123	// TODO: authenticate if needed
124
125	device->FinishedDialing();
126
127	// start decoding
128	int32 length = 0, position = 0;
129	bool inPacket = true, needsEscape = false;
130
131	while(true) {
132		// ignore data if buffer is full
133		if(position == MODEM_MTU)
134			position = 0;
135
136		length = read(handle, buffer + position, MODEM_MTU - position);
137
138		if(length < 0 || !device->IsUp()) {
139			device->ConnectionLost();
140			return B_ERROR;
141		}
142
143		// decode the packet
144		for(int32 index = 0; index < length; ) {
145			if(buffer[position] == FLAG_SEQUENCE) {
146				if(inPacket && position > 0)
147					device->DataReceived(buffer, position);
148						// DataReceived() will check FCS
149
150				length = length - index - 1;
151					// remaining data length
152				memmove(buffer, buffer + position + 1, length);
153				position = index = 0;
154
155				needsEscape = false;
156				inPacket = true;
157				continue;
158			}
159
160			if(buffer[position + index] < 0x20) {
161				++index;
162				continue;
163			}
164
165			if(needsEscape) {
166				buffer[position] = buffer[position + index] ^ 0x20;
167				++position;
168				needsEscape = false;
169			} else if(buffer[position + index] == CONTROL_ESCAPE) {
170				++index;
171				needsEscape = true;
172			} else {
173				buffer[position] = buffer[position + index];
174				++position;
175			}
176		}
177	}
178}
179
180
181ModemDevice::ModemDevice(KPPPInterface& interface, driver_parameter *settings)
182	: KPPPDevice("Modem", 0, interface, settings),
183	fPortName(NULL),
184	fHandle(-1),
185	fWorkerThread(-1),
186	fOutputBytes(0),
187	fState(INITIAL)
188{
189#if DEBUG
190	TRACE("ModemDevice: Constructor\n");
191	if(!settings || !settings->parameters)
192		TRACE("ModemDevice::ctor: No settings!\n");
193#endif
194
195	fACFC = new ACFCHandler(REQUEST_ACFC | ALLOW_ACFC, interface);
196	if(!interface.LCP().AddOptionHandler(fACFC)) {
197		fInitStatus = B_ERROR;
198		return;
199	}
200
201	interface.SetPFCOptions(PPP_REQUEST_PFC | PPP_ALLOW_PFC);
202
203	SetSpeed(19200);
204	SetMTU(MODEM_MTU);
205		// MTU size does not contain PPP header
206
207	fPortName = get_parameter_value(MODEM_PORT_KEY, settings);
208	fInitString = get_parameter_value(MODEM_INIT_KEY, settings);
209	fDialString = get_parameter_value(MODEM_DIAL_KEY, settings);
210
211	TRACE("ModemDevice::ctor: interfaceName: %s\n", fPortName);
212}
213
214
215ModemDevice::~ModemDevice()
216{
217	TRACE("ModemDevice: Destructor\n");
218}
219
220
221status_t
222ModemDevice::InitCheck() const
223{
224	if(fState != INITIAL && Handle() == -1)
225		return B_ERROR;
226
227	return PortName() && InitString() && DialString()
228		&& KPPPDevice::InitCheck() == B_OK ? B_OK : B_ERROR;
229}
230
231
232bool
233ModemDevice::Up()
234{
235	TRACE("ModemDevice: Up()\n");
236
237	if(InitCheck() != B_OK)
238		return false;
239
240	if(IsUp())
241		return true;
242
243	fState = INITIAL;
244		// reset state
245
246	// check if we are allowed to go up now (user intervention might disallow that)
247	if(!UpStarted()) {
248		CloseModem();
249		DownEvent();
250		return true;
251			// there was no error
252	}
253
254	OpenModem();
255
256	fState = DIALING;
257
258	if(fWorkerThread == -1) {
259		fWorkerThread = spawn_kernel_thread(worker_thread, "Modem: worker_thread",
260			B_NORMAL_PRIORITY, this);
261		resume_thread(fWorkerThread);
262	}
263
264	return true;
265}
266
267
268bool
269ModemDevice::Down()
270{
271	TRACE("ModemDevice: Down()\n");
272
273	if(InitCheck() != B_OK)
274		return false;
275
276	fState = TERMINATING;
277
278	if(!IsUp()) {
279		fState = INITIAL;
280		CloseModem();
281		DownEvent();
282		return true;
283	}
284
285	DownStarted();
286		// this tells StateMachine that DownEvent() does not mean we lost connection
287
288	// worker_thread will notice that we are terminating (IsUp() == false)
289	// ConnectionLost() will be called so we can terminate the connection there.
290	int32 tmp;
291	wait_for_thread(fWorkerThread, &tmp);
292
293	DownEvent();
294
295	return true;
296}
297
298
299void
300ModemDevice::SetSpeed(uint32 bps)
301{
302	fInputTransferRate = bps / 8;
303	fOutputTransferRate = (fInputTransferRate * 60) / 100;
304		// 60% of input transfer rate
305}
306
307
308uint32
309ModemDevice::InputTransferRate() const
310{
311	return fInputTransferRate;
312}
313
314
315uint32
316ModemDevice::OutputTransferRate() const
317{
318	return fOutputTransferRate;
319}
320
321
322uint32
323ModemDevice::CountOutputBytes() const
324{
325	return fOutputBytes;
326}
327
328
329void
330ModemDevice::OpenModem()
331{
332	if(Handle() >= 0)
333		return;
334
335	fHandle = open(PortName(), O_RDWR);
336
337	// init port
338	struct termios options;
339	if(ioctl(fHandle, TCGETA, &options) != B_OK) {
340		ERROR("ModemDevice: Could not retrieve port options!\n");
341		return;
342	}
343
344	// adjust options
345	options.c_cflag &= ~CBAUD;
346	options.c_cflag |= B115200;
347	options.c_cflag |= (CLOCAL | CREAD);
348	options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
349	options.c_oflag &= ~OPOST;
350	options.c_cc[VMIN] = 0;
351	options.c_cc[VTIME] = 10;
352
353	// set new options
354	if(ioctl(fHandle, TCSETA, &options) != B_OK) {
355		ERROR("ModemDevice: Could not init port!\n");
356		return;
357	}
358}
359
360
361void
362ModemDevice::CloseModem()
363{
364	if(Handle() >= 0)
365		close(Handle());
366
367	fHandle = -1;
368}
369
370
371void
372ModemDevice::FinishedDialing()
373{
374	fOutputBytes = 0;
375	fState = OPENED;
376	UpEvent();
377}
378
379
380void
381ModemDevice::FailedDialing()
382{
383	fWorkerThread = -1;
384	fState = INITIAL;
385	CloseModem();
386	UpFailedEvent();
387}
388
389
390void
391ModemDevice::ConnectionLost()
392{
393	// switch to command mode and disconnect
394	fWorkerThread = -1;
395	fOutputBytes = 0;
396	snooze(ESCAPE_DELAY);
397	if(write(Handle(), ESCAPE_SEQUENCE, strlen(ESCAPE_SEQUENCE)) < 0)
398		return;
399	snooze(ESCAPE_DELAY);
400
401	modem_put_line(Handle(), AT_HANG_UP, strlen(AT_HANG_UP));
402	CloseModem();
403}
404
405
406status_t
407ModemDevice::Send(struct mbuf *packet, uint16 protocolNumber)
408{
409#if DEBUG
410	TRACE("ModemDevice: Send()\n");
411	dump_packet(packet);
412#endif
413
414	if(!packet)
415		return B_ERROR;
416	else if(InitCheck() != B_OK || protocolNumber != 0) {
417		m_freem(packet);
418		return B_ERROR;
419	} else if(!IsUp()) {
420		m_freem(packet);
421		return PPP_NO_CONNECTION;
422	}
423
424	// we might need room for our header
425	if(fACFC->LocalState() != ACFC_ACCEPTED) {
426		M_PREPEND(packet, 2);
427		if(!packet)
428			return B_ERROR;
429	}
430
431	int32 position = 0, length;
432	if(packet->m_flags & M_PKTHDR)
433		length = packet->m_pkthdr.len;
434	else
435		length = packet->m_len;
436
437	// we need a contiguous chunk of memory
438	packet = m_pullup(packet, length);
439	if(!packet)
440		return B_ERROR;
441
442	uint8 buffer[2 * (MODEM_MTU + PACKET_OVERHEAD)], *data = mtod(packet, uint8*);
443
444	// add header
445	if(fACFC->LocalState() != ACFC_ACCEPTED) {
446		data[0] = ALL_STATIONS;
447		data[1] = UI;
448	}
449
450	// add FCS
451	uint16 fcs = 0xffff;
452	fcs = pppfcs16(fcs, data, length);
453	fcs ^= 0xffff;
454	data[length++] = fcs & 0x00ff;
455	data[length++] = (fcs & 0xff00) >> 8;
456
457	// encode packet
458	buffer[position++] = FLAG_SEQUENCE;
459		// mark beginning of packet
460	for(int32 index = 0; index < length; index++) {
461		if(data[index] < 0x20 || data[index] == FLAG_SEQUENCE
462				|| data[index] == CONTROL_ESCAPE) {
463			buffer[position++] = CONTROL_ESCAPE;
464			buffer[position++] = data[index] ^ 0x20;
465		} else
466			buffer[position++] = data[index];
467	}
468	buffer[position++] = FLAG_SEQUENCE;
469		// mark end of packet
470
471	m_freem(packet);
472
473	// send to modem
474	atomic_add((int32*) &fOutputBytes, position);
475	if(write(Handle(), buffer, position) < 0)
476		return PPP_NO_CONNECTION;
477	atomic_add((int32*) &fOutputBytes, -position);
478
479	return B_OK;
480}
481
482
483status_t
484ModemDevice::DataReceived(uint8 *buffer, uint32 length)
485{
486	// TODO: report corrupted packets to KPPPInterface
487
488	if(length < 3)
489		return B_ERROR;
490
491	// check FCS
492	uint16 fcs = 0xffff;
493	fcs = pppfcs16(fcs, buffer, length - 2);
494	fcs ^= 0xffff;
495	if(buffer[length - 2] != fcs & 0x00ff || buffer[length - 1] != (fcs & 0xff00) >> 8)
496		return B_ERROR;
497
498	if(buffer[0] == ALL_STATIONS && buffer[1] == UI)
499		buffer += 2;
500
501	mbuf *packet = m_gethdr(MT_DATA);
502	packet->m_len = packet->m_pkthdr.len = length - 2;
503	uint8 *data = mtod(packet, uint8*);
504	memcpy(data, buffer, length - 2);
505
506	return Receive(packet);
507}
508
509
510status_t
511ModemDevice::Receive(struct mbuf *packet, uint16 protocolNumber)
512{
513	// we do not need to lock because only the worker_thread calls this method
514
515	if(!packet)
516		return B_ERROR;
517	else if(InitCheck() != B_OK || !IsUp()) {
518		m_freem(packet);
519		return B_ERROR;
520	}
521
522	return Interface().ReceiveFromDevice(packet);
523}
524