1 /* 2 * Copyright (C) ST-Ericsson AB 2010 3 * Author: Sjur Brendeland/sjur.brandeland@stericsson.com 4 * License terms: GNU General Public License (GPL) version 2 5 */ 6 7 #define pr_fmt(fmt) KBUILD_MODNAME ":%s(): " fmt, __func__ 8 9 #include <linux/stddef.h> 10 #include <linux/spinlock.h> 11 #include <linux/slab.h> 12 #include <net/caif/caif_layer.h> 13 #include <net/caif/cfpkt.h> 14 #include <net/caif/cfctrl.h> 15 16 #define container_obj(layr) container_of(layr, struct cfctrl, serv.layer) 17 #define UTILITY_NAME_LENGTH 16 18 #define CFPKT_CTRL_PKT_LEN 20 19 20 21 #ifdef CAIF_NO_LOOP 22 static int handle_loop(struct cfctrl *ctrl, 23 int cmd, struct cfpkt *pkt){ 24 return -1; 25 } 26 #else 27 static int handle_loop(struct cfctrl *ctrl, 28 int cmd, struct cfpkt *pkt); 29 #endif 30 static int cfctrl_recv(struct cflayer *layr, struct cfpkt *pkt); 31 static void cfctrl_ctrlcmd(struct cflayer *layr, enum caif_ctrlcmd ctrl, 32 int phyid); 33 34 35 struct cflayer *cfctrl_create(void) 36 { 37 struct dev_info dev_info; 38 struct cfctrl *this = 39 kmalloc(sizeof(struct cfctrl), GFP_ATOMIC); 40 if (!this) { 41 pr_warn("Out of memory\n"); 42 return NULL; 43 } 44 caif_assert(offsetof(struct cfctrl, serv.layer) == 0); 45 memset(&dev_info, 0, sizeof(dev_info)); 46 dev_info.id = 0xff; 47 memset(this, 0, sizeof(*this)); 48 cfsrvl_init(&this->serv, 0, &dev_info, false); 49 atomic_set(&this->req_seq_no, 1); 50 atomic_set(&this->rsp_seq_no, 1); 51 this->serv.layer.receive = cfctrl_recv; 52 sprintf(this->serv.layer.name, "ctrl"); 53 this->serv.layer.ctrlcmd = cfctrl_ctrlcmd; 54 spin_lock_init(&this->loop_linkid_lock); 55 spin_lock_init(&this->info_list_lock); 56 INIT_LIST_HEAD(&this->list); 57 this->loop_linkid = 1; 58 return &this->serv.layer; 59 } 60 61 static bool param_eq(struct cfctrl_link_param *p1, struct cfctrl_link_param *p2) 62 { 63 bool eq = 64 p1->linktype == p2->linktype && 65 p1->priority == p2->priority && 66 p1->phyid == p2->phyid && 67 p1->endpoint == p2->endpoint && p1->chtype == p2->chtype; 68 69 if (!eq) 70 return false; 71 72 switch (p1->linktype) { 73 case CFCTRL_SRV_VEI: 74 return true; 75 case CFCTRL_SRV_DATAGRAM: 76 return p1->u.datagram.connid == p2->u.datagram.connid; 77 case CFCTRL_SRV_RFM: 78 return 79 p1->u.rfm.connid == p2->u.rfm.connid && 80 strcmp(p1->u.rfm.volume, p2->u.rfm.volume) == 0; 81 case CFCTRL_SRV_UTIL: 82 return 83 p1->u.utility.fifosize_kb == p2->u.utility.fifosize_kb 84 && p1->u.utility.fifosize_bufs == 85 p2->u.utility.fifosize_bufs 86 && strcmp(p1->u.utility.name, p2->u.utility.name) == 0 87 && p1->u.utility.paramlen == p2->u.utility.paramlen 88 && memcmp(p1->u.utility.params, p2->u.utility.params, 89 p1->u.utility.paramlen) == 0; 90 91 case CFCTRL_SRV_VIDEO: 92 return p1->u.video.connid == p2->u.video.connid; 93 case CFCTRL_SRV_DBG: 94 return true; 95 case CFCTRL_SRV_DECM: 96 return false; 97 default: 98 return false; 99 } 100 return false; 101 } 102 103 bool cfctrl_req_eq(struct cfctrl_request_info *r1, 104 struct cfctrl_request_info *r2) 105 { 106 if (r1->cmd != r2->cmd) 107 return false; 108 if (r1->cmd == CFCTRL_CMD_LINK_SETUP) 109 return param_eq(&r1->param, &r2->param); 110 else 111 return r1->channel_id == r2->channel_id; 112 } 113 114 /* Insert request at the end */ 115 void cfctrl_insert_req(struct cfctrl *ctrl, 116 struct cfctrl_request_info *req) 117 { 118 spin_lock(&ctrl->info_list_lock); 119 atomic_inc(&ctrl->req_seq_no); 120 req->sequence_no = atomic_read(&ctrl->req_seq_no); 121 list_add_tail(&req->list, &ctrl->list); 122 spin_unlock(&ctrl->info_list_lock); 123 } 124 125 /* Compare and remove request */ 126 struct cfctrl_request_info *cfctrl_remove_req(struct cfctrl *ctrl, 127 struct cfctrl_request_info *req) 128 { 129 struct cfctrl_request_info *p, *tmp, *first; 130 131 spin_lock(&ctrl->info_list_lock); 132 first = list_first_entry(&ctrl->list, struct cfctrl_request_info, list); 133 134 list_for_each_entry_safe(p, tmp, &ctrl->list, list) { 135 if (cfctrl_req_eq(req, p)) { 136 if (p != first) 137 pr_warn("Requests are not received in order\n"); 138 139 atomic_set(&ctrl->rsp_seq_no, 140 p->sequence_no); 141 list_del(&p->list); 142 goto out; 143 } 144 } 145 p = NULL; 146 out: 147 spin_unlock(&ctrl->info_list_lock); 148 return p; 149 } 150 151 struct cfctrl_rsp *cfctrl_get_respfuncs(struct cflayer *layer) 152 { 153 struct cfctrl *this = container_obj(layer); 154 return &this->res; 155 } 156 157 void cfctrl_set_dnlayer(struct cflayer *this, struct cflayer *dn) 158 { 159 this->dn = dn; 160 } 161 162 void cfctrl_set_uplayer(struct cflayer *this, struct cflayer *up) 163 { 164 this->up = up; 165 } 166 167 static void init_info(struct caif_payload_info *info, struct cfctrl *cfctrl) 168 { 169 info->hdr_len = 0; 170 info->channel_id = cfctrl->serv.layer.id; 171 info->dev_info = &cfctrl->serv.dev_info; 172 } 173 174 void cfctrl_enum_req(struct cflayer *layer, u8 physlinkid) 175 { 176 struct cfctrl *cfctrl = container_obj(layer); 177 int ret; 178 struct cfpkt *pkt = cfpkt_create(CFPKT_CTRL_PKT_LEN); 179 if (!pkt) { 180 pr_warn("Out of memory\n"); 181 return; 182 } 183 caif_assert(offsetof(struct cfctrl, serv.layer) == 0); 184 init_info(cfpkt_info(pkt), cfctrl); 185 cfpkt_info(pkt)->dev_info->id = physlinkid; 186 cfctrl->serv.dev_info.id = physlinkid; 187 cfpkt_addbdy(pkt, CFCTRL_CMD_ENUM); 188 cfpkt_addbdy(pkt, physlinkid); 189 ret = 190 cfctrl->serv.layer.dn->transmit(cfctrl->serv.layer.dn, pkt); 191 if (ret < 0) { 192 pr_err("Could not transmit enum message\n"); 193 cfpkt_destroy(pkt); 194 } 195 } 196 197 int cfctrl_linkup_request(struct cflayer *layer, 198 struct cfctrl_link_param *param, 199 struct cflayer *user_layer) 200 { 201 struct cfctrl *cfctrl = container_obj(layer); 202 u32 tmp32; 203 u16 tmp16; 204 u8 tmp8; 205 struct cfctrl_request_info *req; 206 int ret; 207 char utility_name[16]; 208 struct cfpkt *pkt = cfpkt_create(CFPKT_CTRL_PKT_LEN); 209 if (!pkt) { 210 pr_warn("Out of memory\n"); 211 return -ENOMEM; 212 } 213 cfpkt_addbdy(pkt, CFCTRL_CMD_LINK_SETUP); 214 cfpkt_addbdy(pkt, (param->chtype << 4) + param->linktype); 215 cfpkt_addbdy(pkt, (param->priority << 3) + param->phyid); 216 cfpkt_addbdy(pkt, param->endpoint & 0x03); 217 218 switch (param->linktype) { 219 case CFCTRL_SRV_VEI: 220 break; 221 case CFCTRL_SRV_VIDEO: 222 cfpkt_addbdy(pkt, (u8) param->u.video.connid); 223 break; 224 case CFCTRL_SRV_DBG: 225 break; 226 case CFCTRL_SRV_DATAGRAM: 227 tmp32 = cpu_to_le32(param->u.datagram.connid); 228 cfpkt_add_body(pkt, &tmp32, 4); 229 break; 230 case CFCTRL_SRV_RFM: 231 /* Construct a frame, convert DatagramConnectionID to network 232 * format long and copy it out... 233 */ 234 tmp32 = cpu_to_le32(param->u.rfm.connid); 235 cfpkt_add_body(pkt, &tmp32, 4); 236 /* Add volume name, including zero termination... */ 237 cfpkt_add_body(pkt, param->u.rfm.volume, 238 strlen(param->u.rfm.volume) + 1); 239 break; 240 case CFCTRL_SRV_UTIL: 241 tmp16 = cpu_to_le16(param->u.utility.fifosize_kb); 242 cfpkt_add_body(pkt, &tmp16, 2); 243 tmp16 = cpu_to_le16(param->u.utility.fifosize_bufs); 244 cfpkt_add_body(pkt, &tmp16, 2); 245 memset(utility_name, 0, sizeof(utility_name)); 246 strncpy(utility_name, param->u.utility.name, 247 UTILITY_NAME_LENGTH - 1); 248 cfpkt_add_body(pkt, utility_name, UTILITY_NAME_LENGTH); 249 tmp8 = param->u.utility.paramlen; 250 cfpkt_add_body(pkt, &tmp8, 1); 251 cfpkt_add_body(pkt, param->u.utility.params, 252 param->u.utility.paramlen); 253 break; 254 default: 255 pr_warn("Request setup of bad link type = %d\n", 256 param->linktype); 257 return -EINVAL; 258 } 259 req = kzalloc(sizeof(*req), GFP_KERNEL); 260 if (!req) { 261 pr_warn("Out of memory\n"); 262 return -ENOMEM; 263 } 264 req->client_layer = user_layer; 265 req->cmd = CFCTRL_CMD_LINK_SETUP; 266 req->param = *param; 267 cfctrl_insert_req(cfctrl, req); 268 init_info(cfpkt_info(pkt), cfctrl); 269 /* 270 * NOTE:Always send linkup and linkdown request on the same 271 * device as the payload. Otherwise old queued up payload 272 * might arrive with the newly allocated channel ID. 273 */ 274 cfpkt_info(pkt)->dev_info->id = param->phyid; 275 ret = 276 cfctrl->serv.layer.dn->transmit(cfctrl->serv.layer.dn, pkt); 277 if (ret < 0) { 278 pr_err("Could not transmit linksetup request\n"); 279 cfpkt_destroy(pkt); 280 return -ENODEV; 281 } 282 return 0; 283 } 284 285 int cfctrl_linkdown_req(struct cflayer *layer, u8 channelid, 286 struct cflayer *client) 287 { 288 int ret; 289 struct cfctrl *cfctrl = container_obj(layer); 290 struct cfpkt *pkt = cfpkt_create(CFPKT_CTRL_PKT_LEN); 291 if (!pkt) { 292 pr_warn("Out of memory\n"); 293 return -ENOMEM; 294 } 295 cfpkt_addbdy(pkt, CFCTRL_CMD_LINK_DESTROY); 296 cfpkt_addbdy(pkt, channelid); 297 init_info(cfpkt_info(pkt), cfctrl); 298 ret = 299 cfctrl->serv.layer.dn->transmit(cfctrl->serv.layer.dn, pkt); 300 if (ret < 0) { 301 pr_err("Could not transmit link-down request\n"); 302 cfpkt_destroy(pkt); 303 } 304 return ret; 305 } 306 307 void cfctrl_sleep_req(struct cflayer *layer) 308 { 309 int ret; 310 struct cfctrl *cfctrl = container_obj(layer); 311 struct cfpkt *pkt = cfpkt_create(CFPKT_CTRL_PKT_LEN); 312 if (!pkt) { 313 pr_warn("Out of memory\n"); 314 return; 315 } 316 cfpkt_addbdy(pkt, CFCTRL_CMD_SLEEP); 317 init_info(cfpkt_info(pkt), cfctrl); 318 ret = 319 cfctrl->serv.layer.dn->transmit(cfctrl->serv.layer.dn, pkt); 320 if (ret < 0) 321 cfpkt_destroy(pkt); 322 } 323 324 void cfctrl_wake_req(struct cflayer *layer) 325 { 326 int ret; 327 struct cfctrl *cfctrl = container_obj(layer); 328 struct cfpkt *pkt = cfpkt_create(CFPKT_CTRL_PKT_LEN); 329 if (!pkt) { 330 pr_warn("Out of memory\n"); 331 return; 332 } 333 cfpkt_addbdy(pkt, CFCTRL_CMD_WAKE); 334 init_info(cfpkt_info(pkt), cfctrl); 335 ret = 336 cfctrl->serv.layer.dn->transmit(cfctrl->serv.layer.dn, pkt); 337 if (ret < 0) 338 cfpkt_destroy(pkt); 339 } 340 341 void cfctrl_getstartreason_req(struct cflayer *layer) 342 { 343 int ret; 344 struct cfctrl *cfctrl = container_obj(layer); 345 struct cfpkt *pkt = cfpkt_create(CFPKT_CTRL_PKT_LEN); 346 if (!pkt) { 347 pr_warn("Out of memory\n"); 348 return; 349 } 350 cfpkt_addbdy(pkt, CFCTRL_CMD_START_REASON); 351 init_info(cfpkt_info(pkt), cfctrl); 352 ret = 353 cfctrl->serv.layer.dn->transmit(cfctrl->serv.layer.dn, pkt); 354 if (ret < 0) 355 cfpkt_destroy(pkt); 356 } 357 358 359 void cfctrl_cancel_req(struct cflayer *layr, struct cflayer *adap_layer) 360 { 361 struct cfctrl_request_info *p, *tmp; 362 struct cfctrl *ctrl = container_obj(layr); 363 spin_lock(&ctrl->info_list_lock); 364 pr_warn("enter\n"); 365 366 list_for_each_entry_safe(p, tmp, &ctrl->list, list) { 367 if (p->client_layer == adap_layer) { 368 pr_warn("cancel req :%d\n", p->sequence_no); 369 list_del(&p->list); 370 kfree(p); 371 } 372 } 373 374 spin_unlock(&ctrl->info_list_lock); 375 } 376 377 static int cfctrl_recv(struct cflayer *layer, struct cfpkt *pkt) 378 { 379 u8 cmdrsp; 380 u8 cmd; 381 int ret = -1; 382 u16 tmp16; 383 u8 len; 384 u8 param[255]; 385 u8 linkid; 386 struct cfctrl *cfctrl = container_obj(layer); 387 struct cfctrl_request_info rsp, *req; 388 389 390 cfpkt_extr_head(pkt, &cmdrsp, 1); 391 cmd = cmdrsp & CFCTRL_CMD_MASK; 392 if (cmd != CFCTRL_CMD_LINK_ERR 393 && CFCTRL_RSP_BIT != (CFCTRL_RSP_BIT & cmdrsp)) { 394 if (handle_loop(cfctrl, cmd, pkt) != 0) 395 cmdrsp |= CFCTRL_ERR_BIT; 396 } 397 398 switch (cmd) { 399 case CFCTRL_CMD_LINK_SETUP: 400 { 401 enum cfctrl_srv serv; 402 enum cfctrl_srv servtype; 403 u8 endpoint; 404 u8 physlinkid; 405 u8 prio; 406 u8 tmp; 407 u32 tmp32; 408 u8 *cp; 409 int i; 410 struct cfctrl_link_param linkparam; 411 memset(&linkparam, 0, sizeof(linkparam)); 412 413 cfpkt_extr_head(pkt, &tmp, 1); 414 415 serv = tmp & CFCTRL_SRV_MASK; 416 linkparam.linktype = serv; 417 418 servtype = tmp >> 4; 419 linkparam.chtype = servtype; 420 421 cfpkt_extr_head(pkt, &tmp, 1); 422 physlinkid = tmp & 0x07; 423 prio = tmp >> 3; 424 425 linkparam.priority = prio; 426 linkparam.phyid = physlinkid; 427 cfpkt_extr_head(pkt, &endpoint, 1); 428 linkparam.endpoint = endpoint & 0x03; 429 430 switch (serv) { 431 case CFCTRL_SRV_VEI: 432 case CFCTRL_SRV_DBG: 433 if (CFCTRL_ERR_BIT & cmdrsp) 434 break; 435 /* Link ID */ 436 cfpkt_extr_head(pkt, &linkid, 1); 437 break; 438 case CFCTRL_SRV_VIDEO: 439 cfpkt_extr_head(pkt, &tmp, 1); 440 linkparam.u.video.connid = tmp; 441 if (CFCTRL_ERR_BIT & cmdrsp) 442 break; 443 /* Link ID */ 444 cfpkt_extr_head(pkt, &linkid, 1); 445 break; 446 447 case CFCTRL_SRV_DATAGRAM: 448 cfpkt_extr_head(pkt, &tmp32, 4); 449 linkparam.u.datagram.connid = 450 le32_to_cpu(tmp32); 451 if (CFCTRL_ERR_BIT & cmdrsp) 452 break; 453 /* Link ID */ 454 cfpkt_extr_head(pkt, &linkid, 1); 455 break; 456 case CFCTRL_SRV_RFM: 457 /* Construct a frame, convert 458 * DatagramConnectionID 459 * to network format long and copy it out... 460 */ 461 cfpkt_extr_head(pkt, &tmp32, 4); 462 linkparam.u.rfm.connid = 463 le32_to_cpu(tmp32); 464 cp = (u8 *) linkparam.u.rfm.volume; 465 for (cfpkt_extr_head(pkt, &tmp, 1); 466 cfpkt_more(pkt) && tmp != '\0'; 467 cfpkt_extr_head(pkt, &tmp, 1)) 468 *cp++ = tmp; 469 *cp = '\0'; 470 471 if (CFCTRL_ERR_BIT & cmdrsp) 472 break; 473 /* Link ID */ 474 cfpkt_extr_head(pkt, &linkid, 1); 475 476 break; 477 case CFCTRL_SRV_UTIL: 478 /* Construct a frame, convert 479 * DatagramConnectionID 480 * to network format long and copy it out... 481 */ 482 /* Fifosize KB */ 483 cfpkt_extr_head(pkt, &tmp16, 2); 484 linkparam.u.utility.fifosize_kb = 485 le16_to_cpu(tmp16); 486 /* Fifosize bufs */ 487 cfpkt_extr_head(pkt, &tmp16, 2); 488 linkparam.u.utility.fifosize_bufs = 489 le16_to_cpu(tmp16); 490 /* name */ 491 cp = (u8 *) linkparam.u.utility.name; 492 caif_assert(sizeof(linkparam.u.utility.name) 493 >= UTILITY_NAME_LENGTH); 494 for (i = 0; 495 i < UTILITY_NAME_LENGTH 496 && cfpkt_more(pkt); i++) { 497 cfpkt_extr_head(pkt, &tmp, 1); 498 *cp++ = tmp; 499 } 500 /* Length */ 501 cfpkt_extr_head(pkt, &len, 1); 502 linkparam.u.utility.paramlen = len; 503 /* Param Data */ 504 cp = linkparam.u.utility.params; 505 while (cfpkt_more(pkt) && len--) { 506 cfpkt_extr_head(pkt, &tmp, 1); 507 *cp++ = tmp; 508 } 509 if (CFCTRL_ERR_BIT & cmdrsp) 510 break; 511 /* Link ID */ 512 cfpkt_extr_head(pkt, &linkid, 1); 513 /* Length */ 514 cfpkt_extr_head(pkt, &len, 1); 515 /* Param Data */ 516 cfpkt_extr_head(pkt, ¶m, len); 517 break; 518 default: 519 pr_warn("Request setup - invalid link type (%d)\n", 520 serv); 521 goto error; 522 } 523 524 rsp.cmd = cmd; 525 rsp.param = linkparam; 526 req = cfctrl_remove_req(cfctrl, &rsp); 527 528 if (CFCTRL_ERR_BIT == (CFCTRL_ERR_BIT & cmdrsp) || 529 cfpkt_erroneous(pkt)) { 530 pr_err("Invalid O/E bit or parse error on CAIF control channel\n"); 531 cfctrl->res.reject_rsp(cfctrl->serv.layer.up, 532 0, 533 req ? req->client_layer 534 : NULL); 535 } else { 536 cfctrl->res.linksetup_rsp(cfctrl->serv. 537 layer.up, linkid, 538 serv, physlinkid, 539 req ? req-> 540 client_layer : NULL); 541 } 542 543 if (req != NULL) 544 kfree(req); 545 } 546 break; 547 case CFCTRL_CMD_LINK_DESTROY: 548 cfpkt_extr_head(pkt, &linkid, 1); 549 cfctrl->res.linkdestroy_rsp(cfctrl->serv.layer.up, linkid); 550 break; 551 case CFCTRL_CMD_LINK_ERR: 552 pr_err("Frame Error Indication received\n"); 553 cfctrl->res.linkerror_ind(); 554 break; 555 case CFCTRL_CMD_ENUM: 556 cfctrl->res.enum_rsp(); 557 break; 558 case CFCTRL_CMD_SLEEP: 559 cfctrl->res.sleep_rsp(); 560 break; 561 case CFCTRL_CMD_WAKE: 562 cfctrl->res.wake_rsp(); 563 break; 564 case CFCTRL_CMD_LINK_RECONF: 565 cfctrl->res.restart_rsp(); 566 break; 567 case CFCTRL_CMD_RADIO_SET: 568 cfctrl->res.radioset_rsp(); 569 break; 570 default: 571 pr_err("Unrecognized Control Frame\n"); 572 goto error; 573 break; 574 } 575 ret = 0; 576 error: 577 cfpkt_destroy(pkt); 578 return ret; 579 } 580 581 static void cfctrl_ctrlcmd(struct cflayer *layr, enum caif_ctrlcmd ctrl, 582 int phyid) 583 { 584 struct cfctrl *this = container_obj(layr); 585 switch (ctrl) { 586 case _CAIF_CTRLCMD_PHYIF_FLOW_OFF_IND: 587 case CAIF_CTRLCMD_FLOW_OFF_IND: 588 spin_lock(&this->info_list_lock); 589 if (!list_empty(&this->list)) { 590 pr_debug("Received flow off in control layer\n"); 591 } 592 spin_unlock(&this->info_list_lock); 593 break; 594 default: 595 break; 596 } 597 } 598 599 #ifndef CAIF_NO_LOOP 600 static int handle_loop(struct cfctrl *ctrl, int cmd, struct cfpkt *pkt) 601 { 602 static int last_linkid; 603 u8 linkid, linktype, tmp; 604 switch (cmd) { 605 case CFCTRL_CMD_LINK_SETUP: 606 spin_lock(&ctrl->loop_linkid_lock); 607 for (linkid = last_linkid + 1; linkid < 255; linkid++) 608 if (!ctrl->loop_linkused[linkid]) 609 goto found; 610 for (linkid = last_linkid - 1; linkid > 0; linkid--) 611 if (!ctrl->loop_linkused[linkid]) 612 goto found; 613 spin_unlock(&ctrl->loop_linkid_lock); 614 pr_err("Out of link-ids\n"); 615 return -EINVAL; 616 found: 617 if (!ctrl->loop_linkused[linkid]) 618 ctrl->loop_linkused[linkid] = 1; 619 620 last_linkid = linkid; 621 622 cfpkt_add_trail(pkt, &linkid, 1); 623 spin_unlock(&ctrl->loop_linkid_lock); 624 cfpkt_peek_head(pkt, &linktype, 1); 625 if (linktype == CFCTRL_SRV_UTIL) { 626 tmp = 0x01; 627 cfpkt_add_trail(pkt, &tmp, 1); 628 cfpkt_add_trail(pkt, &tmp, 1); 629 } 630 break; 631 632 case CFCTRL_CMD_LINK_DESTROY: 633 spin_lock(&ctrl->loop_linkid_lock); 634 cfpkt_peek_head(pkt, &linkid, 1); 635 ctrl->loop_linkused[linkid] = 0; 636 spin_unlock(&ctrl->loop_linkid_lock); 637 break; 638 default: 639 break; 640 } 641 return 0; 642 } 643 #endif 644