@@ -351,17 +351,154 @@ int cfctrl_cancel_req(struct cflayer *layr, struct cflayer *adap_layer)
351
351
return found ;
352
352
}
353
353
354
+ static int cfctrl_link_setup (struct cfctrl * cfctrl , struct cfpkt * pkt , u8 cmdrsp )
355
+ {
356
+ u8 len ;
357
+ u8 linkid = 0 ;
358
+ enum cfctrl_srv serv ;
359
+ enum cfctrl_srv servtype ;
360
+ u8 endpoint ;
361
+ u8 physlinkid ;
362
+ u8 prio ;
363
+ u8 tmp ;
364
+ u8 * cp ;
365
+ int i ;
366
+ struct cfctrl_link_param linkparam ;
367
+ struct cfctrl_request_info rsp , * req ;
368
+
369
+ memset (& linkparam , 0 , sizeof (linkparam ));
370
+
371
+ tmp = cfpkt_extr_head_u8 (pkt );
372
+
373
+ serv = tmp & CFCTRL_SRV_MASK ;
374
+ linkparam .linktype = serv ;
375
+
376
+ servtype = tmp >> 4 ;
377
+ linkparam .chtype = servtype ;
378
+
379
+ tmp = cfpkt_extr_head_u8 (pkt );
380
+ physlinkid = tmp & 0x07 ;
381
+ prio = tmp >> 3 ;
382
+
383
+ linkparam .priority = prio ;
384
+ linkparam .phyid = physlinkid ;
385
+ endpoint = cfpkt_extr_head_u8 (pkt );
386
+ linkparam .endpoint = endpoint & 0x03 ;
387
+
388
+ switch (serv ) {
389
+ case CFCTRL_SRV_VEI :
390
+ case CFCTRL_SRV_DBG :
391
+ if (CFCTRL_ERR_BIT & cmdrsp )
392
+ break ;
393
+ /* Link ID */
394
+ linkid = cfpkt_extr_head_u8 (pkt );
395
+ break ;
396
+ case CFCTRL_SRV_VIDEO :
397
+ tmp = cfpkt_extr_head_u8 (pkt );
398
+ linkparam .u .video .connid = tmp ;
399
+ if (CFCTRL_ERR_BIT & cmdrsp )
400
+ break ;
401
+ /* Link ID */
402
+ linkid = cfpkt_extr_head_u8 (pkt );
403
+ break ;
404
+
405
+ case CFCTRL_SRV_DATAGRAM :
406
+ linkparam .u .datagram .connid = cfpkt_extr_head_u32 (pkt );
407
+ if (CFCTRL_ERR_BIT & cmdrsp )
408
+ break ;
409
+ /* Link ID */
410
+ linkid = cfpkt_extr_head_u8 (pkt );
411
+ break ;
412
+ case CFCTRL_SRV_RFM :
413
+ /* Construct a frame, convert
414
+ * DatagramConnectionID
415
+ * to network format long and copy it out...
416
+ */
417
+ linkparam .u .rfm .connid = cfpkt_extr_head_u32 (pkt );
418
+ cp = (u8 * ) linkparam .u .rfm .volume ;
419
+ for (tmp = cfpkt_extr_head_u8 (pkt );
420
+ cfpkt_more (pkt ) && tmp != '\0' ;
421
+ tmp = cfpkt_extr_head_u8 (pkt ))
422
+ * cp ++ = tmp ;
423
+ * cp = '\0' ;
424
+
425
+ if (CFCTRL_ERR_BIT & cmdrsp )
426
+ break ;
427
+ /* Link ID */
428
+ linkid = cfpkt_extr_head_u8 (pkt );
429
+
430
+ break ;
431
+ case CFCTRL_SRV_UTIL :
432
+ /* Construct a frame, convert
433
+ * DatagramConnectionID
434
+ * to network format long and copy it out...
435
+ */
436
+ /* Fifosize KB */
437
+ linkparam .u .utility .fifosize_kb = cfpkt_extr_head_u16 (pkt );
438
+ /* Fifosize bufs */
439
+ linkparam .u .utility .fifosize_bufs = cfpkt_extr_head_u16 (pkt );
440
+ /* name */
441
+ cp = (u8 * ) linkparam .u .utility .name ;
442
+ caif_assert (sizeof (linkparam .u .utility .name )
443
+ >= UTILITY_NAME_LENGTH );
444
+ for (i = 0 ; i < UTILITY_NAME_LENGTH && cfpkt_more (pkt ); i ++ ) {
445
+ tmp = cfpkt_extr_head_u8 (pkt );
446
+ * cp ++ = tmp ;
447
+ }
448
+ /* Length */
449
+ len = cfpkt_extr_head_u8 (pkt );
450
+ linkparam .u .utility .paramlen = len ;
451
+ /* Param Data */
452
+ cp = linkparam .u .utility .params ;
453
+ while (cfpkt_more (pkt ) && len -- ) {
454
+ tmp = cfpkt_extr_head_u8 (pkt );
455
+ * cp ++ = tmp ;
456
+ }
457
+ if (CFCTRL_ERR_BIT & cmdrsp )
458
+ break ;
459
+ /* Link ID */
460
+ linkid = cfpkt_extr_head_u8 (pkt );
461
+ /* Length */
462
+ len = cfpkt_extr_head_u8 (pkt );
463
+ /* Param Data */
464
+ cfpkt_extr_head (pkt , NULL , len );
465
+ break ;
466
+ default :
467
+ pr_warn ("Request setup, invalid type (%d)\n" , serv );
468
+ return -1 ;
469
+ }
470
+
471
+ rsp .cmd = CFCTRL_CMD_LINK_SETUP ;
472
+ rsp .param = linkparam ;
473
+ spin_lock_bh (& cfctrl -> info_list_lock );
474
+ req = cfctrl_remove_req (cfctrl , & rsp );
475
+
476
+ if (CFCTRL_ERR_BIT == (CFCTRL_ERR_BIT & cmdrsp ) ||
477
+ cfpkt_erroneous (pkt )) {
478
+ pr_err ("Invalid O/E bit or parse error "
479
+ "on CAIF control channel\n" );
480
+ cfctrl -> res .reject_rsp (cfctrl -> serv .layer .up , 0 ,
481
+ req ? req -> client_layer : NULL );
482
+ } else {
483
+ cfctrl -> res .linksetup_rsp (cfctrl -> serv .layer .up , linkid ,
484
+ serv , physlinkid ,
485
+ req ? req -> client_layer : NULL );
486
+ }
487
+
488
+ kfree (req );
489
+
490
+ spin_unlock_bh (& cfctrl -> info_list_lock );
491
+
492
+ return 0 ;
493
+ }
494
+
354
495
static int cfctrl_recv (struct cflayer * layer , struct cfpkt * pkt )
355
496
{
356
497
u8 cmdrsp ;
357
498
u8 cmd ;
358
- int ret = -1 ;
359
- u8 len ;
360
- u8 param [255 ];
499
+ int ret = 0 ;
361
500
u8 linkid = 0 ;
362
501
struct cfctrl * cfctrl = container_obj (layer );
363
- struct cfctrl_request_info rsp , * req ;
364
-
365
502
366
503
cmdrsp = cfpkt_extr_head_u8 (pkt );
367
504
cmd = cmdrsp & CFCTRL_CMD_MASK ;
@@ -374,150 +511,7 @@ static int cfctrl_recv(struct cflayer *layer, struct cfpkt *pkt)
374
511
375
512
switch (cmd ) {
376
513
case CFCTRL_CMD_LINK_SETUP :
377
- {
378
- enum cfctrl_srv serv ;
379
- enum cfctrl_srv servtype ;
380
- u8 endpoint ;
381
- u8 physlinkid ;
382
- u8 prio ;
383
- u8 tmp ;
384
- u8 * cp ;
385
- int i ;
386
- struct cfctrl_link_param linkparam ;
387
- memset (& linkparam , 0 , sizeof (linkparam ));
388
-
389
- tmp = cfpkt_extr_head_u8 (pkt );
390
-
391
- serv = tmp & CFCTRL_SRV_MASK ;
392
- linkparam .linktype = serv ;
393
-
394
- servtype = tmp >> 4 ;
395
- linkparam .chtype = servtype ;
396
-
397
- tmp = cfpkt_extr_head_u8 (pkt );
398
- physlinkid = tmp & 0x07 ;
399
- prio = tmp >> 3 ;
400
-
401
- linkparam .priority = prio ;
402
- linkparam .phyid = physlinkid ;
403
- endpoint = cfpkt_extr_head_u8 (pkt );
404
- linkparam .endpoint = endpoint & 0x03 ;
405
-
406
- switch (serv ) {
407
- case CFCTRL_SRV_VEI :
408
- case CFCTRL_SRV_DBG :
409
- if (CFCTRL_ERR_BIT & cmdrsp )
410
- break ;
411
- /* Link ID */
412
- linkid = cfpkt_extr_head_u8 (pkt );
413
- break ;
414
- case CFCTRL_SRV_VIDEO :
415
- tmp = cfpkt_extr_head_u8 (pkt );
416
- linkparam .u .video .connid = tmp ;
417
- if (CFCTRL_ERR_BIT & cmdrsp )
418
- break ;
419
- /* Link ID */
420
- linkid = cfpkt_extr_head_u8 (pkt );
421
- break ;
422
-
423
- case CFCTRL_SRV_DATAGRAM :
424
- linkparam .u .datagram .connid =
425
- cfpkt_extr_head_u32 (pkt );
426
- if (CFCTRL_ERR_BIT & cmdrsp )
427
- break ;
428
- /* Link ID */
429
- linkid = cfpkt_extr_head_u8 (pkt );
430
- break ;
431
- case CFCTRL_SRV_RFM :
432
- /* Construct a frame, convert
433
- * DatagramConnectionID
434
- * to network format long and copy it out...
435
- */
436
- linkparam .u .rfm .connid =
437
- cfpkt_extr_head_u32 (pkt );
438
- cp = (u8 * ) linkparam .u .rfm .volume ;
439
- for (tmp = cfpkt_extr_head_u8 (pkt );
440
- cfpkt_more (pkt ) && tmp != '\0' ;
441
- tmp = cfpkt_extr_head_u8 (pkt ))
442
- * cp ++ = tmp ;
443
- * cp = '\0' ;
444
-
445
- if (CFCTRL_ERR_BIT & cmdrsp )
446
- break ;
447
- /* Link ID */
448
- linkid = cfpkt_extr_head_u8 (pkt );
449
-
450
- break ;
451
- case CFCTRL_SRV_UTIL :
452
- /* Construct a frame, convert
453
- * DatagramConnectionID
454
- * to network format long and copy it out...
455
- */
456
- /* Fifosize KB */
457
- linkparam .u .utility .fifosize_kb =
458
- cfpkt_extr_head_u16 (pkt );
459
- /* Fifosize bufs */
460
- linkparam .u .utility .fifosize_bufs =
461
- cfpkt_extr_head_u16 (pkt );
462
- /* name */
463
- cp = (u8 * ) linkparam .u .utility .name ;
464
- caif_assert (sizeof (linkparam .u .utility .name )
465
- >= UTILITY_NAME_LENGTH );
466
- for (i = 0 ;
467
- i < UTILITY_NAME_LENGTH
468
- && cfpkt_more (pkt ); i ++ ) {
469
- tmp = cfpkt_extr_head_u8 (pkt );
470
- * cp ++ = tmp ;
471
- }
472
- /* Length */
473
- len = cfpkt_extr_head_u8 (pkt );
474
- linkparam .u .utility .paramlen = len ;
475
- /* Param Data */
476
- cp = linkparam .u .utility .params ;
477
- while (cfpkt_more (pkt ) && len -- ) {
478
- tmp = cfpkt_extr_head_u8 (pkt );
479
- * cp ++ = tmp ;
480
- }
481
- if (CFCTRL_ERR_BIT & cmdrsp )
482
- break ;
483
- /* Link ID */
484
- linkid = cfpkt_extr_head_u8 (pkt );
485
- /* Length */
486
- len = cfpkt_extr_head_u8 (pkt );
487
- /* Param Data */
488
- cfpkt_extr_head (pkt , & param , len );
489
- break ;
490
- default :
491
- pr_warn ("Request setup, invalid type (%d)\n" ,
492
- serv );
493
- goto error ;
494
- }
495
-
496
- rsp .cmd = cmd ;
497
- rsp .param = linkparam ;
498
- spin_lock_bh (& cfctrl -> info_list_lock );
499
- req = cfctrl_remove_req (cfctrl , & rsp );
500
-
501
- if (CFCTRL_ERR_BIT == (CFCTRL_ERR_BIT & cmdrsp ) ||
502
- cfpkt_erroneous (pkt )) {
503
- pr_err ("Invalid O/E bit or parse error "
504
- "on CAIF control channel\n" );
505
- cfctrl -> res .reject_rsp (cfctrl -> serv .layer .up ,
506
- 0 ,
507
- req ? req -> client_layer
508
- : NULL );
509
- } else {
510
- cfctrl -> res .linksetup_rsp (cfctrl -> serv .
511
- layer .up , linkid ,
512
- serv , physlinkid ,
513
- req ? req ->
514
- client_layer : NULL );
515
- }
516
-
517
- kfree (req );
518
-
519
- spin_unlock_bh (& cfctrl -> info_list_lock );
520
- }
514
+ ret = cfctrl_link_setup (cfctrl , pkt , cmdrsp );
521
515
break ;
522
516
case CFCTRL_CMD_LINK_DESTROY :
523
517
linkid = cfpkt_extr_head_u8 (pkt );
@@ -544,9 +538,9 @@ static int cfctrl_recv(struct cflayer *layer, struct cfpkt *pkt)
544
538
break ;
545
539
default :
546
540
pr_err ("Unrecognized Control Frame\n" );
541
+ ret = -1 ;
547
542
goto error ;
548
543
}
549
- ret = 0 ;
550
544
error :
551
545
cfpkt_destroy (pkt );
552
546
return ret ;
0 commit comments