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