Infinity loop - waterfilling algorithm

I'm trying to write a code to carry out the Waterfilling algorithm for MIMO channels. I've written the code below but there is something wrong with my WHILE loop. I get negative power and also endless loop at the end. I'm trying to write the code for the given formula below:

and here is my code:

P=zeros(1,r);
sq=zeros(1,3);

for i=1:numel(SNR)
    
    p=1;
    go = true;
    while go
    
        for j=1:r-p+1
            K(j)=1/L(j);
            T =sum(K);
        end;
        m =(1/(r-p+1))*(1+ (1/0.1)*T);
        for j=1:r-p+1 
            P(j)= m-(1/(0.1*L(j)));
            if P(r-p+1)< 0
            P(r-p+1)=0;
            end;
    
            sq(j) = sum(P);
            if sq(j) <= 1.00005
                go = false;
            else
                p=p+1;
            end;
        end;
    end;
    for j=1:r
        C_equal_t2(j)=log2(1+SNR(i)*L(j)*P(j));
        C_eigen(i)=sum(C_equal_t2);
    end;
end;

[Geoff Hayes]

Ahmad - in your code you have

        for j=1:r-p+1 
            P(j)= m-(1/(0.1*L(j)));
            if P(r-p+1)< 0
                P(r-p+1)=0;
            end;
            % etc.
        end

Why, on each iteration of the loop, do you check to see if

        P(r-p+1)< 0

? You are doing the same comparison for each j. Do you mean this to be

            if P(j) < 0
                P(j)=0;
            end;

instead?

[Ahmad Halimi]

No. Actually for each iteration it's needed to check just the last P which is indicated by P(r-p+1)

the SNR range is [0.01 to 100] and the problem is that for low values the loop never ends, it's ok for large values like 20 as an example