1 //----------------------------------------------------------------------- 2 // This software is part of the OpenBeOS distribution and is covered 3 // by the OpenBeOS license. 4 // 5 // Copyright (c) 2003-2004 Waldemar Kornewald, Waldemar.Kornewald@web.de 6 //----------------------------------------------------------------------- 7 8 #include <cstdio> 9 10 #include "ModemDevice.h" 11 #include "ACFCHandler.h" 12 #include "fcs.h" 13 14 #include <core_funcs.h> 15 #include <unistd.h> 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 dprintf("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 dprintf("%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 fInterfaceName(NULL), 185 fHandle(-1), 186 fWorkerThread(-1), 187 fOutputBytes(0), 188 fState(INITIAL) 189 { 190 #if DEBUG 191 dprintf("ModemDevice: Constructor\n"); 192 if(!settings || !settings->parameters) 193 dprintf("ModemDevice::ctor: No settings!\n"); 194 #endif 195 196 fACFC = new ACFCHandler(REQUEST_ACFC | ALLOW_ACFC, interface); 197 if(!interface.LCP().AddOptionHandler(fACFC)) { 198 fInitStatus = B_ERROR; 199 return; 200 } 201 202 interface.SetPFCOptions(PPP_REQUEST_PFC | PPP_ALLOW_PFC); 203 204 SetSpeed(19200); 205 SetMTU(MODEM_MTU); 206 // MTU size does not contain PPP header 207 208 fInterfaceName = get_parameter_value(MODEM_INTERFACE_KEY, settings); 209 fInitString = get_parameter_value(MODEM_INIT_KEY, settings); 210 fDialString = get_parameter_value(MODEM_DIAL_KEY, settings); 211 212 #if DEBUG 213 dprintf("ModemDevice::ctor: interfaceName: %s\n", fInterfaceName); 214 #endif 215 } 216 217 218 ModemDevice::~ModemDevice() 219 { 220 #if DEBUG 221 dprintf("ModemDevice: Destructor\n"); 222 #endif 223 } 224 225 226 status_t 227 ModemDevice::InitCheck() const 228 { 229 if(fState != INITIAL && Handle() == -1) 230 return B_ERROR; 231 232 return InterfaceName() && KPPPDevice::InitCheck() == B_OK ? B_OK : B_ERROR; 233 } 234 235 236 bool 237 ModemDevice::Up() 238 { 239 #if DEBUG 240 dprintf("ModemDevice: Up()\n"); 241 #endif 242 243 if(InitCheck() != B_OK) 244 return false; 245 246 LockerHelper locker(fLock); 247 248 if(IsUp()) 249 return true; 250 251 fState = INITIAL; 252 // reset state 253 254 // check if we are allowed to go up now (user intervention might disallow that) 255 if(!UpStarted()) { 256 CloseModem(); 257 DownEvent(); 258 return true; 259 // there was no error 260 } 261 262 OpenModem(); 263 264 fState = DIALING; 265 266 if(fWorkerThread == -1) { 267 fWorkerThread = spawn_kernel_thread(worker_thread, "Modem: worker_thread", 268 B_NORMAL_PRIORITY, this); 269 resume_thread(fWorkerThread); 270 } 271 272 return true; 273 } 274 275 276 bool 277 ModemDevice::Down() 278 { 279 #if DEBUG 280 dprintf("ModemDevice: Down()\n"); 281 #endif 282 283 if(InitCheck() != B_OK) 284 return false; 285 286 LockerHelper locker(fLock); 287 288 fState = TERMINATING; 289 290 if(!IsUp()) { 291 fState = INITIAL; 292 CloseModem(); 293 DownEvent(); 294 return true; 295 } 296 297 DownStarted(); 298 // this tells StateMachine that DownEvent() does not mean we lost connection 299 300 // worker_thread will notice that we are terminating (IsUp() == false) 301 // ConnectionLost() will be called so we can terminate the connection there. 302 locker.UnlockNow(); 303 int32 tmp; 304 wait_for_thread(fWorkerThread, &tmp); 305 306 DownEvent(); 307 308 return true; 309 } 310 311 312 void 313 ModemDevice::SetSpeed(uint32 bps) 314 { 315 fInputTransferRate = bps / 8; 316 fOutputTransferRate = (fInputTransferRate * 60) / 100; 317 // 60% of input transfer rate 318 } 319 320 321 uint32 322 ModemDevice::InputTransferRate() const 323 { 324 return fInputTransferRate; 325 } 326 327 328 uint32 329 ModemDevice::OutputTransferRate() const 330 { 331 return fOutputTransferRate; 332 } 333 334 335 uint32 336 ModemDevice::CountOutputBytes() const 337 { 338 return fOutputBytes; 339 } 340 341 342 void 343 ModemDevice::OpenModem() 344 { 345 LockerHelper locker(fLock); 346 347 if(Handle() >= 0) 348 return; 349 350 char path[B_PATH_NAME_LENGTH]; 351 sprintf(path, "/dev/modem/%s", InterfaceName()); 352 fHandle = open(path, O_RDWR); 353 } 354 355 356 void 357 ModemDevice::CloseModem() 358 { 359 LockerHelper locker(fLock); 360 361 if(Handle() >= 0) 362 close(Handle()); 363 364 fHandle = -1; 365 } 366 367 368 void 369 ModemDevice::FinishedDialing() 370 { 371 LockerHelper locker(fLock); 372 373 fOutputBytes = 0; 374 fState = OPENED; 375 UpEvent(); 376 } 377 378 379 void 380 ModemDevice::FailedDialing() 381 { 382 LockerHelper locker(fLock); 383 384 fWorkerThread = -1; 385 fState = INITIAL; 386 CloseModem(); 387 UpFailedEvent(); 388 } 389 390 391 void 392 ModemDevice::ConnectionLost() 393 { 394 LockerHelper locker(fLock); 395 396 // switch to command mode and disconnect 397 fWorkerThread = -1; 398 fOutputBytes = 0; 399 snooze(ESCAPE_DELAY); 400 if(write(Handle(), ESCAPE_SEQUENCE, strlen(ESCAPE_SEQUENCE)) < 0) 401 return; 402 snooze(ESCAPE_DELAY); 403 404 modem_put_line(Handle(), AT_HANG_UP, strlen(AT_HANG_UP)); 405 CloseModem(); 406 } 407 408 409 status_t 410 ModemDevice::Send(struct mbuf *packet, uint16 protocolNumber = 0) 411 { 412 #if DEBUG 413 dprintf("ModemDevice: Send()\n"); 414 dump_packet(packet); 415 #endif 416 417 if(!packet) 418 return B_ERROR; 419 else if(InitCheck() != B_OK || protocolNumber != 0) { 420 m_freem(packet); 421 return B_ERROR; 422 } else if(!IsUp()) { 423 m_freem(packet); 424 return PPP_NO_CONNECTION; 425 } 426 427 // we might need room for our header 428 if(fACFC->LocalState() != ACFC_ACCEPTED) { 429 M_PREPEND(packet, 2); 430 if(!packet) 431 return B_ERROR; 432 } 433 434 int32 position = 0, length; 435 if(packet->m_flags & M_PKTHDR) 436 length = packet->m_pkthdr.len; 437 else 438 length = packet->m_len; 439 440 // we need a contiguous chunk of memory 441 packet = m_pullup(packet, length); 442 if(!packet) 443 return B_ERROR; 444 445 uint8 buffer[2 * (MODEM_MTU + PACKET_OVERHEAD)], *data = mtod(packet, uint8*); 446 447 // add header 448 if(fACFC->LocalState() != ACFC_ACCEPTED) { 449 data[0] = ALL_STATIONS; 450 data[1] = UI; 451 } 452 453 // add FCS 454 uint16 fcs = 0xffff; 455 fcs = pppfcs16(fcs, data, length); 456 fcs ^= 0xffff; 457 data[length++] = fcs & 0x00ff; 458 data[length++] = (fcs & 0xff00) >> 8; 459 460 // encode packet 461 buffer[position++] = FLAG_SEQUENCE; 462 // mark beginning of packet 463 for(int32 index = 0; index < length; index++) { 464 if(data[index] < 0x20 || data[index] == FLAG_SEQUENCE 465 || data[index] == CONTROL_ESCAPE) { 466 buffer[position++] = CONTROL_ESCAPE; 467 buffer[position++] = data[index] ^ 0x20; 468 } else 469 buffer[position++] = data[index]; 470 } 471 buffer[position++] = FLAG_SEQUENCE; 472 // mark end of packet 473 474 m_freem(packet); 475 476 // send to modem 477 atomic_add((int32*) &fOutputBytes, position); 478 if(write(Handle(), buffer, position) < 0) 479 return PPP_NO_CONNECTION; 480 atomic_add((int32*) &fOutputBytes, -position); 481 482 return B_OK; 483 } 484 485 486 status_t 487 ModemDevice::DataReceived(uint8 *buffer, uint32 length) 488 { 489 // TODO: report corrupted packets to KPPPInterface 490 491 if(length < 3) 492 return B_ERROR; 493 494 // check FCS 495 uint16 fcs = 0xffff; 496 fcs = pppfcs16(fcs, buffer, length - 2); 497 fcs ^= 0xffff; 498 if(buffer[length - 2] != fcs & 0x00ff || buffer[length - 1] != (fcs & 0xff00) >> 8) 499 return B_ERROR; 500 501 if(buffer[0] == ALL_STATIONS && buffer[1] == UI) 502 buffer += 2; 503 504 mbuf *packet = m_gethdr(MT_DATA); 505 packet->m_len = packet->m_pkthdr.len = length - 2; 506 uint8 *data = mtod(packet, uint8*); 507 memcpy(data, buffer, length - 2); 508 509 return Receive(packet); 510 } 511 512 513 status_t 514 ModemDevice::Receive(struct mbuf *packet, uint16 protocolNumber = 0) 515 { 516 // we do not need to lock because only the worker_thread calls this method 517 518 if(!packet) 519 return B_ERROR; 520 else if(InitCheck() != B_OK || !IsUp()) { 521 m_freem(packet); 522 return B_ERROR; 523 } 524 525 return Interface().ReceiveFromDevice(packet); 526 } 527