swi_setusbcomp: working on MC7710 ...

1 files changed, 301 insertions, 79 deletions

diff --git a/scripts/swi_setusbcomp.pl b/scripts/swi_setusbcomp.pl
index 8903b6e..f819221 100644..100755
--- a/scripts/swi_setusbcomp.pl
+++ b/scripts/swi_setusbcomp.pl
@@ -6,17 +6,29 @@ use strict;
 use warnings;
 use Data::Dumper;
 use Getopt::Long;
-use Encode;
-use Devel::SimpleTrace;
 use UUID::Tiny ':std';
+use IPC::Shareable;
+use Fcntl ':mode';
+use File::Basename;
+use JSON;
 
 my $maxctrl = 4096; # default, will be overridden by ioctl if supported
 my $mgmt = "/dev/cdc-wdm0";
 my $debug;
-
+my $verbose = 1;
+my $usbcomp;
+ 
+# a few global variables
+my $lastmbim = 0;
+my $lastqmi;
+my $dmscid;
+my $tid = 1;
+    
 GetOptions(
+    'usbcomp=i' => \$usbcomp,
     'device=s' => \$mgmt,
     'debug!' => \$debug,
+    'verbose!' => \$verbose,
     'help|h|?' => \&usage,
     ) || &usage;
 
@@ -101,14 +113,7 @@ sub mk_open_msg {
     my $buf = &init_msg_header(1); # MBIM_OPEN_MSG  
     $buf = &_push($buf, "V", $maxctrl); # MaxControlTransfer 
 
-    if ($debug) {
-	my $n = length($buf);
-	warn("[" . localtime . "] sending $n bytes to $mgmt\n");
-	print "\n---\n";
-	printf "%02x " x $n, unpack("C*", $buf);
-	print "\n---\n";
-    }
-
+    printf "MBIM>: " . "%02x " x length($buf) . "\n", unpack("C*", $buf) if $debug;
     return $buf;
 }
 
@@ -116,14 +121,7 @@ sub mk_open_msg {
 sub mk_close_msg {
     my $buf = &init_msg_header(2); # MBIM_CLOSE_MSG  
 
-    if ($debug) {
-	my $n = length($buf);
-	warn("[" . localtime . "] sending $n bytes to $mgmt\n");
-	print "\n---\n";
-	printf "%02x " x $n, unpack("C*", $buf);
-	print "\n---\n";
-    }
-
+    printf "MBIM>: " . "%02x " x length($buf) . "\n", unpack("C*", $buf) if $debug;
     return $buf;
 }
 
@@ -142,14 +140,7 @@ sub mk_command_msg {
 		  length($info), # InformationBufferLength  
 	);
     $buf = &_push($buf, "a*", $info);  # InformationBuffer  
-
-    if ($debug) {
-	my $n = length($buf);
-	warn("[" . localtime . "] sending $n bytes to $mgmt\n");
-	print "\n---\n";
-	printf "%02x " x $n, unpack("C*", $buf);
-	print "\n---\n";
-    }
+    printf "MBIM>: " . "%02x " x length($buf) . "\n", unpack("C*", $buf) if $debug;
     return $buf;
 }
 
@@ -157,49 +148,121 @@ sub decode_mbim {
     my $msg = shift;
     my ($type, $len, $tid) = unpack("VVV", $msg);
 
-    print "MBIM_MESSAGE_HEADER\n";
-    printf "  MessageType:\t0x%08x\n", $type;
-    printf "  MessageLength:\t%d\n", $len;
-    printf "  TransactionId:\t%d\n", $tid;
+    if ($debug) {
+	print "MBIM_MESSAGE_HEADER\n";
+	printf "  MessageType:\t0x%08x\n", $type;
+	printf "  MessageLength:\t%d\n", $len;
+	printf "  TransactionId:\t%d\n", $tid;
+    }
     if ($type == 0x80000001 || $type == 0x80000002) { # MBIM_OPEN_DONE ||  MBIM_CLOSE_DONE 
 	my $status = unpack("V", substr($msg, 12));
-	printf "  Status:\t0x%08x\n", $status;
+	printf "  Status:\t0x%08x\n", $status if $debug;
     } elsif ($type == 0x80000003) { # MBIM_COMMAND_DONE 
 	my ($total, $current) = unpack("VV", substr($msg, 12)); # FragmentHeader  
-	print "MBIM_FRAGMENT_HEADER\n";
-	printf "  TotalFragments:\t0x%08x\n", $total;
-	printf "  CurrentFragment:\t0x%08x\n", $current;
-
+	if ($debug) {
+	    print "MBIM_FRAGMENT_HEADER\n";
+	    printf "  TotalFragments:\t0x%08x\n", $total;
+	    printf "  CurrentFragment:\t0x%08x\n", $current;
+	}
 	my $uuid = uuid_to_string(substr($msg, 20, 16));
 	my $service = &uuid_to_service($uuid);
-	print "$service ($uuid)\n";
+	print "$service ($uuid)\n"  if $debug;
 
 	my ($cid, $status, $infolen) = unpack("VVV", substr($msg, 36));
 	my $info = substr($msg, 48);
-	printf "  CID:\t0x%08x\n", $cid;
-	printf "  Status:\t0x%08x\n", $status;
-	print "InformationBuffer [$infolen]:\n";
+	if ($debug) {
+	    printf "  CID:\t\t0x%08x\n", $cid;
+	    printf "  Status:\t0x%08x\n", $status;
+	    print "InformationBuffer [$infolen]:\n";
+	}
 	if ($infolen != length($info)) {
-	    print "Fragmented data is not supported\n";
+	    print "Fragmented MBIM transactions are not supported\n";
 	} elsif ($service eq "EXT_QMUX") {
-	    print Dumper(&decode_qmi($info));
+	    # save the decoded QMI message
+	    $lastqmi = &decode_qmi($info);
 	}
 	# silently ignoring InformationBuffer payload of other services
     }
     # ignoring all other types of MBIM messages
+    
+    # save message type
+    $lastmbim = $type;   
+}
+
+# read from F until timeout
+sub read_mbim {
+    my $timeout = shift || 0;
+    my $found = undef;
+
+    eval {
+	local $SIG{ALRM} = sub { die "alarm\n" }; # NB: \n required
+	my $raw = '';
+	my $msglen = 0;
+	alarm $timeout;
+	do {
+	    my $len = 0;
+	    if ($len < 3 || $len < $msglen) {
+		my $tmp;
+		my $n = sysread(F, $tmp, $maxctrl);
+		if ($n) {
+		    $len = $n;
+		    $raw = $tmp;
+		    printf "MBIM<: " . "%02x " x $n . "\n", unpack("C*", $tmp) if $debug;
+		} else {
+		    $found = 1;
+		}
+	    }
+
+	    # get expected message length
+	    $msglen = unpack("V", substr($raw, 4, 4));
+
+	    if ($len >= $msglen) {
+		$len -= $msglen;
+		&decode_mbim(substr($raw, 0, $msglen));
+		$raw = substr($raw, $msglen);
+		$msglen = 0;
+		$found = 1;
+	    } else {
+		warn "$len < $msglen\n";
+	    }
+	} while (!$found);
+	alarm 0;
+    };
+    if ($@) {
+	die unless $@ =~ /^alarm/;   # propagate unexpected errors
+    }
 }
 
 ### QMI helpers ###
+my %sysname = (
+    0    => "QMI_CTL",
+    1    => "QMI_WDS",
+    2    => "QMI_DMS",
+    3    => "QMI_NAS",
+    4    => "QMI_QOS",
+    5    => "QMI_WMS",
+    6    => "QMI_PDS",
+    7    => "QMI_AUTH",
+    8    => "QMI_AT",
+    9    => "QMI_VOICE",
+    0xa  => "QMI_CAT2",
+    0xb  => "QMI UIM",
+    0xc  => "QMI PBM",
+    0xe  => "QMI RMTFS",
+    0x10 => "QMI_LOC",
+    0x11 => "QMI_SAR",
+    0x14 => "QMI_CSD",
+    0x15 => "QMI_EFS",
+    0x17 => "QMI_TS",
+    0x18 => "QMI_TMD",
+    0x1a => "QMI_WDA",
+    0x1e => "QMI_QCMAP",
+    0x24 => "QMI_PDC",
+    0xe0 => "QMI_CAT", # duplicate!
+    0xe1 => "QMI_RMS",
+    0xe2 => "QMI_OMA",
+    );
 
-use constant {
-    QMI_CTL => 0x00,
-    QMI_WDS => 0x01,
-    QMI_DMS => 0x02,
-    QMI_NAS => 0x03,
-    QMI_WMS => 0x05,
-    QMI_PDS => 0x06,
-    QMI_LOC => 0x10,
-};
 
 # $tlvs = { type1 => packdata, type2 => packdata, .. 
 sub mk_qmi {
@@ -211,10 +274,10 @@ sub mk_qmi {
 	$tlvbytes .= pack("Cv", $tlv, length($tlvs->{$tlv})) . $tlvs->{$tlv};
     }
     my $tlvlen = length($tlvbytes);
-    if ($sys != QMI_CTL) {
+    if ($sys != 0) {
 	return pack("CvCCCCvvv", 1, 12 + $tlvlen, 0, $sys, $cid, 0, $tid++, $msgid, $tlvlen) . $tlvbytes;
     } else {
-	return pack("CvCCCCCvv", 1, 11 + $tlvlen, 0, QMI_CTL, 0, 0, $tid++, $msgid, $tlvlen) . $tlvbytes;
+	return pack("CvCCCCCvv", 1, 11 + $tlvlen, 0, 0, 0, 0, $tid++, $msgid, $tlvlen) . $tlvbytes;
     }
 }
 
@@ -229,9 +292,9 @@ sub decode_qmi {
     return {} unless ($ret->{tf} == 1);
 
     # tid is 1 byte for QMI_CTL and 2 bytes for the others...
-    @$ret{'flags','tid','msgid','tlvlen'} = unpack($ret->{sys} == QMI_CTL ? "CCvv" : "Cvvv" , substr($packet, 6));
+    @$ret{'flags','tid','msgid','tlvlen'} = unpack($ret->{sys} == 0 ? "CCvv" : "Cvvv" , substr($packet, 6));
     my $tlvlen = $ret->{'tlvlen'};
-    my $tlvs = substr($packet, $ret->{'sys'} == QMI_CTL ? 12 : 13 );
+    my $tlvs = substr($packet, $ret->{'sys'} == 0 ? 12 : 13 );
 
     # add the tlvs
      while ($tlvlen > 0) {
@@ -243,11 +306,84 @@ sub decode_qmi {
     return $ret;
 }
 
+sub qmiver {
+    my $qmi = shift;
+
+    # decode the list of supported systems in TLV 0x01
+    my @data = @{$qmi->{'tlvs'}{0x01}};
+    my $n = shift(@data);
+    my $data = pack("C*", @data);
+    print "supports $n QMI subsystems:\n";
+    for (my $i = 0; $i < $n; $i++) {
+	my ($sys, $maj, $min) = unpack("Cvv", $data);
+	my $system = $sysname{$sys} || sprintf("%#04x", $sys);
+	print "  $system ($maj.$min)\n";
+	$data = substr($data, 5);
+    }
+}
+
+sub qmiok {
+    my $qmi = shift;
+    return exists($qmi->{tlvs}{0x02}) && (unpack("v", pack("C*", @{$qmi->{tlvs}{0x02}}[2..3])) == 0);
+}
+
+sub do_qmi {
+    my $msgid = shift;
+    my $qmi = shift;
+
+    printf "QMI>: " . "%02x " x length($qmi) . "\n", unpack("C*", $qmi) if $debug;
+    print F &mk_command_msg('EXT_QMUX', 1, 1, $qmi);
+    my $count = 5; # seconds timeout
+    while (!($lastmbim == 0x80000003 && ref($lastqmi) && ($lastqmi->{'msgid'} == $msgid))) {
+	sleep(1);
+	return undef if (!$count--); # timeout
+    }
+    my $status = &qmiok($lastqmi);
+    printf "QMI msg '0x%04x' returned status = $status\n", $msgid if $verbose;
+    print to_json($lastqmi) if ($debug && !$status);
+    return $status;
+}
+
+
+## Sierra USB comp
+my %comps = (
+    0  => 'HIP  DM    NMEA  AT    MDM1  MDM2  MDM3  MS',
+    1  => 'HIP  DM    NMEA  AT    MDM1  MS',
+    2  => 'HIP  DM    NMEA  AT    NIC1  MS',
+    3  => 'HIP  DM    NMEA  AT    MDM1  NIC1  MS',
+    4  => 'HIP  DM    NMEA  AT    NIC1  NIC2  NIC3  MS',
+    5  => 'HIP  DM    NMEA  AT    ECM1  MS',
+    6  => 'DM   NMEA  AT    QMI',
+    7  => 'DM   NMEA  AT    RMNET1 RMNET2 RMNET3',
+    8  => 'DM   NMEA  AT    MBIM',
+    9  => 'MBIM',
+    10 => 'NMEA MBIM',
+    11 => 'DM   MBIM',
+    12 => 'DM   NMEA  MBIM',
+    13 => 'Config1: comp6    Config2: comp8',
+    14 => 'Config1: comp6    Config2: comp9',
+    15 => 'Config1: comp6    Config2: comp10',
+    16 => 'Config1: comp6    Config2: comp11',
+    17 => 'Config1: comp6    Config2: comp12',
+    18 => 'Config1: comp7    Config2: comp8',
+    19 => 'Config1: comp7    Config2: comp9',
+    20 => 'Config1: comp7    Config2: comp10',
+    21 => 'Config1: comp7    Config2: comp11',
+    22 => 'Config1: comp7    Config2: comp12',
+);
+
 ### main ###
 
+# verify that the $mgmt device is a chardev provided by the cdc_mbim driver
+my ($mode, $rdev) = (stat($mgmt))[2,6];
+die "'$mgmt' is not a character device\n" unless S_ISCHR($mode);
+my $driver = basename(readlink(sprintf("/sys/dev/char/%u:%u/device/driver", $rdev >> 8, $rdev & 0xff)));
+die "'$mgmt' is provided by '$driver' - only MBIM devices are supported\n" unless ($driver eq "cdc_mbim");
+
 # open device now and keep it open until exit
 open(F, "+<", $mgmt) || die "open $mgmt: $!\n";
 autoflush F 1;
+autoflush STDOUT 1;
 
 # check message size
 require 'sys/ioctl.ph';
@@ -261,58 +397,144 @@ if ($r) {
 }
 print "MaxMessageSize=$maxctrl\n"  if $debug;
 
-print F &mk_open_msg;
-# wait for OPEN
+# fork the reader
+my $pid = fork();
+if ($pid == 0) { # child
 
+    # allow writer to see the last MBIM message
+    tie $lastmbim, 'IPC::Shareable', 'mbim', { create => 1, destroy => 0 } || die "tie failed\n";
+    tie $lastqmi, 'IPC::Shareable', 'qmi', { create => 1, destroy => 0 } || die "tie failed\n";
 
-# test QMI
+    # reset to avoid inheriting old values...
+    $lastmbim = 0;
 
-# FIXME:  simpler to import a QMI encoder, than to manually fix up all these static arrays. And so much more readable...
+    # loop until CLOSE_DONE...
+    while ($lastmbim != 0x80000002) {
+	&read_mbim(60);
+    }
+    $lastmbim = 0x80000002; # in case of timeout...
+    print "exiting reader\n" if $debug;
+    exit 0;
+} elsif (!$pid) {
+    die "fork() failed: $!\n";
+}
 
-# QMI_CTL_MESSAGE_GET_VERSION_INFO
-print F &mk_command_msg('EXT_QMUX', 1, 1, &mk_qmi(0, 0, 0x0021, { 0x01 => 0xff }));
+# watch reader status
+tie $lastmbim, 'IPC::Shareable', 'mbim', { create => 1, destroy => 1 } || die "tie failed\n";
+tie $lastqmi, 'IPC::Shareable', 'qmi', { create => 1, destroy => 1 } || die "tie failed\n";
 
-# wait for response (and decode?)
+# reset to avoid inheriting old values...
+$lastmbim = 0;
 
-# allocate a DMS CID (or just reuse the one allocated by the MBIM firmware application?)
-# QMI_CTL_GET_CLIENT_ID, TLV 0x01 => 2 (DMS)
-print F &mk_command_msg('EXT_QMUX', 1, 1, &mk_qmi(0, 0, 0x0022, { 0x01 => 2 }));
+# send OPEN and wait until reader has seen the OPEN_DONE message
+print F &mk_open_msg;
 
-# wait for response and decode
-# => $dmscid
+# wait for OPEN_DONE
+while ($lastmbim != 0x80000001) {
+    sleep(1);
+}
+print "MBIM OPEN succeeded\n" if $verbose;
 
+# verify QMI channel support with QMI_CTL_MESSAGE_GET_VERSION_INFO
+unless (&do_qmi(0x0021, &mk_qmi(0, 0, 0x0021, { 0x01 => pack("C", 255), }))) {
+    print "Failed to verify QMI vendor specific MBIM service\n";
+    &quit;
+}
+print "MBIM QMI support verified\n";
+&qmiver($lastqmi) if $verbose;
 
-#QMI_DMS_SWI_SETUSBCOMP (or whatever)
+# allocate a DMS CID (or just reuse the one allocated by the MBIM firmware application?)
+# QMI_CTL_GET_CLIENT_ID, TLV 0x01 => 2 (DMS)
+unless (&do_qmi(0x0022, &mk_qmi(0, 0, 0x0022, { 0x01 => pack("C", 2), }))) {
+    print "Failed to get QMI DMS client ID\n";
+    &quit;
+}
+$dmscid = $lastqmi->{'tlvs'}{0x01}[1]; # save the DMS CID
+print "Got QMI DMS client ID '$dmscid'\n" if $verbose;
 
+#QMI_DMS_SWI_SETUSBCOMP (or whatever)
 # get USB comp = 0x555B
 # set USB comp = 0x555C
 # "Set FCC Authentication" =  0x555F
 ##print F &mk_command_msg('EXT_QMUX', 1, 1,  &mk_qmi(2, $dmscid, 0x555c, { 0x01 => $usbcomp}));
 # wait for response and decode
 
-#let's test a get first, eh?
-print F &mk_command_msg('EXT_QMUX', 1, 1,  &mk_qmi(2, $dmscid, 0x555b));
-# wait for response and decode
+# always get first.  We need the list of supported settings to allow set
+&do_qmi(0x555b, &mk_qmi(2, $dmscid, 0x555b, {})) || &quit;
+my $current = $lastqmi->{'tlvs'}{0x10}[0];
+my @supported = sort(@{$lastqmi->{'tlvs'}{0x11}});
+my $count = hex(shift(@supported));
+
+# basic sanity:
+if ($count != $#supported) {
+    print "ERROR: array length mismatch, $count != $#supported\n";
+    print to_json(\@supported),"\n";
+    &quit;
+}
 
-# release DMS CID
-# QMI_CTL_RELEASE_CLIENT_ID
-print F &mk_command_msg('EXT_QMUX', 1, 1, &mk_qmi(0, 0, 0x0022, { 0x01 =>  pack("C*", 2, $dmscid)}));
-# wait for response and decode
 
-print F &mk_close_msg;
-# wait for response, close and exit
+&quit unless (grep { $current == $_ } @supported); # verify that the current comp is supported
+
+# dump current settings
+printf "Current USB composition: %d\n", $current;
+if ($verbose) {
+    print "USB compositions:\n";
+    for my $i (sort { $a <=> $b } keys %comps) {
+	printf "%s %2i - %-48s %sSUPPORTED\n", $i == $current ? '*' : ' ', $i, $comps{$i}, (grep { $i == $_ } @supported) ? '' : 'NOT ';
+    }
+}
 
-# close device
-close(F);
+# want a new setting?
+&quit unless defined($usbcomp);
 
+# no need to change to the current setting
+if ($usbcomp == $current) {
+    print "Current setting is already '$usbcomp'\n";
+    &quit;
+}
 
+# verify that the new setting is supported
+unless (grep { $usbcomp == $_ } @supported) {
+    print "USB composition '$usbcomp' is not supported\n";
+    &quit;
+}
 
+# attempt to change USB comp
+if (!&do_qmi(0x555c, &mk_qmi(2, $dmscid, 0x555c, { 0x01 => pack("C", $usbcomp)}))) {
+    print "Failed to change USB composition to '$usbcomp'\n";
+}
+
+&quit;
+
+sub quit {
+    if ($dmscid) {
+	# release DMS CID
+	# QMI_CTL_RELEASE_CLIENT_ID
+	&do_qmi(0x0023, &mk_qmi(0, 0, 0x0023, { 0x01 =>  pack("C*", 2, $dmscid)}));
+    }
+
+    # send CLOSE
+    print F &mk_close_msg;
+
+    # wait for the reader to exit (on CLOSE_DONE)
+    waitpid($pid, 0);
+
+    close(F);
+    exit 0; # will exit parent
+}
+    
 sub usage {
     print STDERR <<EOH
 Usage: $0 [options]  
 
 Where [options] are
+  --device=<mbimdev>    use <mbimdev> (default: '$mgmt')
+  --usbcomp=<num>	change USB composition setting
+  --debug		enable verbose debug output
+  --help		this help text
 
+  The current setting and supported modes will always be displayed
+  
 
 EOH
     ;