PIN Change (EE2021)

public async Task<(bool ApplicationCrytogramVerificationResult, byte[] ArpcResponse)> CardUnblockAsync(string PAN, string sequenceNumber, byte smiKeyIndex, byte smcKeyIndex, byte acKeyIndex,
            byte[] applicationCryptogram, byte[] ATC)
        {
            HsmTcpConnection connection = null;
            try
            {
                if (string.IsNullOrEmpty(PAN) || string.IsNullOrEmpty(sequenceNumber))
                    throw new HsmApplicationCryptogramVerifyException("PAN Data is not valid");

                if (applicationCryptogram == null)
                    throw new HsmApplicationCryptogramVerifyException("Application Cryptogram is not valid");

                if (applicationCryptogram == null || applicationCryptogram.Length != 8)
                    throw new HsmApplicationCryptogramVerifyException("Application Cryptogram is not valid");

                if (ATC == null)
                    throw new HsmApplicationCryptogramVerifyException("ATC is not valid");

                int msgLength = 6 + //header
                                3 + //function code
                                1 + //function modifier
                                1 + //function flag
                                3 + //IMK SMI
                                3 + //IMK SMC
                                1 + //MK method
                                1 + 8 // PAN + Sequence number
                                + 1 //SM Key Method

                                + 1 + 8 // SM Key Length + data
                                + 2 // PF method + length
                                + 8 // encrypted pin block
                                + 3 // PPK
                                + 1 // PIN Block Format
                                + 6 // ANB
                                + 4 // PC and PI Data
                                + 2 + 7 + 8 + ATC.Length + applicationCryptogram.Length;
          //    int msgLength = 6 + 3 + 1 + 1 + 1 + 3 + 3 + 1 + 1 + (((PAN.Length + sequenceNumber.Length) /2) + ((PAN.Length+sequenceNumber.Length) %2)) + 1 + applicationCryptogram.Length + 8 + 8 + 3 + 1 + 6 + 2 + 7 + ATC.Length + applicationCryptogram.Length;

                //header + function name + function modifier + function flag + scheme + SMI + SMC + IMKAC + (Len +PAN + SequenceNos) + (Len + AC) + (OLD & New PIN) + PPK
                //+ PinBlockFormat + ANB + ScriptDataPosition + Script Data

                byte[] req = new byte[msgLength];
                req[0] = 0x01; // SOH Character
                req[1] = 0x01; // Version Number

                var seqNum = GetNextSequenceNumber();
                req[2] = (byte)(seqNum >> 8); // Sequence Number
                req[3] = (byte)seqNum; // Sequence Number

                req[4] = (byte)((msgLength - 6) >> 8); // length of message (header is not included that is why -6)
                req[5] = (byte)(msgLength - 6); // length of message

                req[6] = 0xEE; // Function name
                req[7] = 0x20;
                req[8] = 0x21;

                req[9] = 0x00; // Function modifier
                req[10] = 0x03; //Function flag - 00= Pin Unblock only, 03= Pin Change

                req[11] = 0x02; // Var Len
                req[12] = 0x00;
                req[13] = smiKeyIndex; // Key specifier for IMK SMI

                req[14] = 0x02; // Var Len
                req[15] = 0x00;
                req[16] = smcKeyIndex; // Key specifier for IMK SMC

                req[17] = 0x00; // MK Method

                req[18] = (byte) Convertor.StringToBCD(PAN + sequenceNumber,2, PAN.Length,req,19);

                var pos = 19 + req[18];

                //SM Key method
                req[pos++] = 0x05;

                //SM key data includes ATC plus empty data
                req[pos++] = 0x08; //length
                req[pos++] = 0x00;
                req[pos++] = 0x19;
                req[pos++] = 0x00;
                req[pos++] = 0x00;
                req[pos++] = 0x00;
                req[pos++] = 0x00;
                req[pos++] = 0x00;
                req[pos++] = 0x00;

                req[pos++] = 0x02; //PF method
                req[pos++] = 0x12; //Length of PF Method

                req[pos++] = 0x32; //encrypted pin block
                req[pos++] = 0x13;
                req[pos++] = 0x17;
                req[pos++] = 0x5A;
                req[pos++] = 0xBC;
                req[pos++] = 0x32;
                req[pos++] = 0xB9;
                req[pos++] = 0x49;

                req[pos++] = 0x02;
                req[pos++] = 0x00;
                req[pos++] = 0x07; //PPK. we should not be using this for this function - index 7 is staging , 2 is live

                req[pos++] = 0x10; //PIN Block Format

                //Account Number Block
                pos += Convertor.StringToBCD(PAN, PAN.Length - 13, 12, req, pos);//ANB

                req[pos++] = 0x00; // PC method
                req[pos++] = 0x00; // length of PC method
                req[pos++] = 0x00; // PI method

                req[pos++] = 0x1B; // Length of PI Data

                req[pos++] = 0x00; //Script Data Position
                req[pos++] = 0x0F;

                req[pos++] = 0x18; //Script data len
                req[pos++] = 0x84; //Class
                req[pos++] = 0x24; //Ins
                req[pos++] = 0x00; //P1
                req[pos++] = 0x00;// P2
                req[pos++] = 0x08; //Lc

                Buffer.BlockCopy(ATC, 0, req, pos, ATC.Length);
                pos += ATC.Length;

                Buffer.BlockCopy(applicationCryptogram, 0, req, pos, applicationCryptogram.Length);
                pos += applicationCryptogram.Length;

                req[pos++] = 0x00; //8 bytes used for Pin change only
                req[pos++] = 0x00;
                req[pos++] = 0x00;
                req[pos++] = 0x00;
                req[pos++] = 0x00;
                req[pos++] = 0x00;
                req[pos++] = 0x00;
                req[pos++] = 0x00;

                req[pos++] = 0x80;
                Console.WriteLine($"Request Buffer: {ByteArrayToString(req)}");

                connection = GetConnection();
                var response = await connection.SendRequestAsync(req);

                Console.WriteLine($"Response Buffer: {ByteArrayToString(response)}");


                if (response == null || response.Length < 4)
                    throw new HsmApplicationCryptogramVerifyException("EMV-PIN-CHANGE-UNBLOCK (EE2016) function returned invalid response");


                //check response code
                if (response[3] != 0x00 && response[3] != 0x08) //function failed
                    throw new HsmApplicationCryptogramVerifyException(string.Format("EMV-PIN-CHANGE-UNBLOCK (EE2016) response is {0:X2}", response[3]));
                else if (response[3] == 0x08)
                {
                    return (false, null);
                }
                else //0x00
                {
                    _log.Debug("EMV-PIN-CHANGE-UNBLOCK (EE2016) succeeded");
                    byte[] ARPC = new byte[response[4]];
                    Buffer.BlockCopy(response, 5, ARPC, 0, response[4]);
                    return (true, ARPC);
                }
            }
            finally
            {
                //return connection to stack
                if (hsmConnections != null)
                    hsmConnections.Push(connection);
            }
        }