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