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 22 static char sDigits[] = "0123456789ABCDEF"; 23 void 24 dump_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 49 status_t 50 modem_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 61 status_t 62 modem_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 92 static 93 status_t 94 worker_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 181 ModemDevice::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 215 ModemDevice::~ModemDevice() 216 { 217 TRACE("ModemDevice: Destructor\n"); 218 } 219 220 221 status_t 222 ModemDevice::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 232 bool 233 ModemDevice::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 268 bool 269 ModemDevice::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 299 void 300 ModemDevice::SetSpeed(uint32 bps) 301 { 302 fInputTransferRate = bps / 8; 303 fOutputTransferRate = (fInputTransferRate * 60) / 100; 304 // 60% of input transfer rate 305 } 306 307 308 uint32 309 ModemDevice::InputTransferRate() const 310 { 311 return fInputTransferRate; 312 } 313 314 315 uint32 316 ModemDevice::OutputTransferRate() const 317 { 318 return fOutputTransferRate; 319 } 320 321 322 uint32 323 ModemDevice::CountOutputBytes() const 324 { 325 return fOutputBytes; 326 } 327 328 329 void 330 ModemDevice::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 361 void 362 ModemDevice::CloseModem() 363 { 364 if(Handle() >= 0) 365 close(Handle()); 366 367 fHandle = -1; 368 } 369 370 371 void 372 ModemDevice::FinishedDialing() 373 { 374 fOutputBytes = 0; 375 fState = OPENED; 376 UpEvent(); 377 } 378 379 380 void 381 ModemDevice::FailedDialing() 382 { 383 fWorkerThread = -1; 384 fState = INITIAL; 385 CloseModem(); 386 UpFailedEvent(); 387 } 388 389 390 void 391 ModemDevice::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 406 status_t 407 ModemDevice::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 483 status_t 484 ModemDevice::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 510 status_t 511 ModemDevice::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