Add FPLMN read and program

Change-Id: I9ce8c1af691c28ea9ed69e7b5f03f0c02d1f029b
diff --git a/pySim-prog.py b/pySim-prog.py
index 448f2a0..7c644b7 100755
--- a/pySim-prog.py
+++ b/pySim-prog.py
@@ -150,6 +150,9 @@
                       default=None,
                       choices=['{:02X}'.format(int(m)) for m in EF_AD.OP_MODE],
                       )
+    parser.add_option("-f", "--fplmn", dest="fplmn", action="append",
+                      help="Set Forbidden PLMN. Add multiple time for multiple FPLMNS",
+                      )
     parser.add_option("--epdgid", dest="epdgid",
                       help="Set Home Evolved Packet Data Gateway (ePDG) Identifier. (Only FQDN format supported)",
                       )
@@ -498,6 +501,7 @@
         'impi': opts.impi,
         'impu': opts.impu,
         'opmode': opts.opmode,
+        'fplmn': opts.fplmn,
     }
 
 
diff --git a/pySim-read.py b/pySim-read.py
index b7fe1f2..ddc193a 100755
--- a/pySim-read.py
+++ b/pySim-read.py
@@ -253,6 +253,14 @@
             else:
                 print("EHPLMN: Can't read, response code = %s" % (sw,))
 
+        # EF.FPLMN
+        if usim_card.file_exists(EF_USIM_ADF_map['FPLMN']):
+            res, sw = usim_card.read_fplmn()
+            if sw == '9000':
+                print(f'FPLMN:\n{res}')
+            else:
+                print(f'FPLMN: Can\'t read, response code = {sw}')
+
         # EF.UST
         try:
             if usim_card.file_exists(EF_USIM_ADF_map['UST']):
diff --git a/pySim/cards.py b/pySim/cards.py
index 0f34f9b..d3a43f3 100644
--- a/pySim/cards.py
+++ b/pySim/cards.py
@@ -381,6 +381,22 @@
         data, sw = self._scc.update_binary(EF_USIM_ADF_map['EHPLMN'], ehplmn)
         return sw
 
+    def read_fplmn(self):
+        res, sw = self._scc.read_binary(EF_USIM_ADF_map['FPLMN'])
+        if sw == '9000':
+            return format_xplmn(res), sw
+        else:
+            return None, sw
+
+    def update_fplmn(self, fplmn):
+        self._scc.select_file('3f00')
+        self.select_adf_by_aid('USIM')
+        size = self._scc.binary_size(EF_USIM_ADF_map['FPLMN'])
+        encoded = ''.join([enc_plmn(plmn[:3], plmn[3:]) for plmn in fplmn])
+        encoded = rpad(encoded, size)
+        data, sw = self._scc.update_binary(EF_USIM_ADF_map['FPLMN'], encoded)
+        return sw
+
     def read_epdgid(self):
         (res, sw) = self._scc.read_binary(EF_USIM_ADF_map['ePDGId'])
         if sw == '9000':
@@ -1702,6 +1718,7 @@
             'smsp': self.update_smsp,
             'ki': self.update_ki,
             'opc': self.update_opc,
+            'fplmn': self.update_fplmn,
         }
 
     @classmethod
@@ -1715,6 +1732,7 @@
         return None
 
     def program(self, p):
+        self.set_apdu_parameter('00', '0004')
         # Authenticate
         self._scc.verify_chv(0xc, h2b('3834373936313533'))
         for handler in self._program_handlers:
diff --git a/pysim-testdata/Fairwaves-SIM.ok b/pysim-testdata/Fairwaves-SIM.ok
index f07c033..063fc6b 100644
--- a/pysim-testdata/Fairwaves-SIM.ok
+++ b/pysim-testdata/Fairwaves-SIM.ok
@@ -88,6 +88,18 @@
 	Service 58 - Extension 8
 	Service 59 - MMS User Connectivity Parameters
 
+FPLMN:
+	ffffff # unused
+	ffffff # unused
+	ffffff # unused
+	ffffff # unused
+	ffffff # unused
+	ffffff # unused
+	ffffff # unused
+	ffffff # unused
+	ffffff # unused
+	ffffff # unused
+
 USIM Service Table: 01ea1ffc21360480010000
 	Service 1 - Local Phone Book
 	Service 10 - Short Message Storage (SMS)
diff --git a/pysim-testdata/Wavemobile-SIM.ok b/pysim-testdata/Wavemobile-SIM.ok
index d8f1a6f..980332f 100644
--- a/pysim-testdata/Wavemobile-SIM.ok
+++ b/pysim-testdata/Wavemobile-SIM.ok
@@ -93,6 +93,12 @@
 	Service 58 - Extension 8
 	Service 59 - MMS User Connectivity Parameters
 
+FPLMN:
+	ffffff # unused
+	ffffff # unused
+	ffffff # unused
+	ffffff # unused
+
 USIM Service Table: 9eff1b3c37fe5900000000
 	Service 2 - Fixed Dialling Numbers (FDN)
 	Service 3 - Extension 2
diff --git a/pysim-testdata/sysmoISIM-SJA2.ok b/pysim-testdata/sysmoISIM-SJA2.ok
index a8c98d9..f9476df 100644
--- a/pysim-testdata/sysmoISIM-SJA2.ok
+++ b/pysim-testdata/sysmoISIM-SJA2.ok
@@ -108,6 +108,12 @@
 	ffffff # unused
 	ffffff # unused
 
+FPLMN:
+	ffffff # unused
+	ffffff # unused
+	ffffff # unused
+	ffffff # unused
+
 USIM Service Table: beff9f9de73e0408400170330000002e00000000
 	Service 2 - Fixed Dialling Numbers (FDN)
 	Service 3 - Extension 2
diff --git a/pysim-testdata/sysmoUSIM-SJS1.ok b/pysim-testdata/sysmoUSIM-SJS1.ok
index 14d7110..b4277f4 100644
--- a/pysim-testdata/sysmoUSIM-SJS1.ok
+++ b/pysim-testdata/sysmoUSIM-SJS1.ok
@@ -104,6 +104,12 @@
 	Service 58 - Extension 8
 	Service 59 - MMS User Connectivity Parameters
 
+FPLMN:
+	62f201 # MCC: 262 MNC: 010
+	62f202 # MCC: 262 MNC: 020
+	62f203 # MCC: 262 MNC: 030
+	62f207 # MCC: 262 MNC: 070
+
 USIM Service Table: 9e6b1dfc67f6580000
 	Service 2 - Fixed Dialling Numbers (FDN)
 	Service 3 - Extension 2